/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
/*
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* Author: Brian Gerkey */
#include "amcl_node.h"
#include <qdebug.h>
#include "frames.hpp"
#include "filteringalgorithm.h"
#define D_LASER_LOSS_MAX_TIME 60.0
#define D_AMCL_POSITION_INITIAL_COUNT 7


static double
normalize(double z)
{
  return atan2(sin(z),cos(z));
}
static double
angle_diff(double a, double b)
{
  double d1, d2;
  a = normalize(a);
  b = normalize(b);
  d1 = a-b;
  d2 = 2*M_PI - fabs(d1);
  if(d1 > 0)
    d2 *= -1.0;
  if(fabs(d1) < fabs(d2))
    return(d1);
  else
    return(d2);
}

static const std::string scan_topic_ = "front_filter_laser_scan";//hualei


std::vector<std::pair<int,int> > AmclNode::free_space_indices;

#define USAGE "USAGE: amcl"

boost::shared_ptr<AmclNode> amcl_node_ptr;

void sigintHandler(int sig)
{
  // Save latest pose as we're shutting down.
  amcl_node_ptr->savePoseToServer();
  ros::shutdown();
}

//int
//main(int argc, char** argv)
//{
//  ros::init(argc, argv, "amcl");
//  ros::NodeHandle nh;

//  // Override default sigint handler
//  signal(SIGINT, sigintHandler);

//  // Make our node available to sigintHandler
//  amcl_node_ptr.reset(new AmclNode());

//  if (argc == 1)
//  {
//    // run using ROS input
//    ros::spin();
//  }
//  else if ((argc == 3) && (std::string(argv[1]) == "--run-from-bag"))
//  {
//    amcl_node_ptr->runFromBag(argv[2]);
//  }

//  // Without this, our boost locks are not shut down nicely
//  amcl_node_ptr.reset();

//  // To quote Morgan, Hooray!
//  return(0);
//}

AmclNode::AmclNode(AmclConfig *configParameterIn) :
        sent_first_transform_(false),
        latest_tf_valid_(false),
        map_(NULL),
        pf_(NULL),
        resample_count_(0),
        odom_(NULL),
        laser_(NULL),
	      private_nh_("~"),
        initial_pose_hyp_(NULL),
        first_map_received_(false),
        first_reconfigure_call_(true)
{
  boost::recursive_mutex::scoped_lock l(configuration_mutex_);
  positionCertaintyFactor=0;
  isUpdatePartialEveryTime=0;
  isLaserReceiveOk=true;
  isPfOk=true;
  minConsumed=100000000000;
  currentConsumed=0;
  maxConsumed=0;
  isEnableToWork=false;//主程序启动太慢会导致激光数据接收超时异常
  configParameter=configParameterIn;
  extRequestCount=0;
  laserRequestCount=0;
  noCheckCount=0;
  isLaserReadyForCalculate=false;
  isUseRosLaserMsg=false;
  isCalculateLaser=true;

  reflectPosition.isUpdateSucess=false;
  amclPosition.isUpdateSucess=false;
  debugInfo.isCorrectSucess=0;
  debugInfo.correctCount=0;
  debugFlag=0;

  frame_baselink_in_laserlink=new KDL::Frame;
  frame_laserlink_in_baselink=new KDL::Frame;
  frame_baselink_in_odom=new KDL::Frame;
  frame_baselink_in_odom_old=new KDL::Frame;
  frame_odom_in_map=new KDL::Frame;
//  frame_correct==new KDL::Frame;

  filtering_position_x=new FilteringAlgorithm;
  filtering_position_y=new FilteringAlgorithm;
  filtering_position_r=new FilteringAlgorithm;

  initialPositionCount=D_AMCL_POSITION_INITIAL_COUNT;

  initialAgvPositionCount=50;//开机后强制初始化位置. 5次太少了，可能位置不能初始化成功。

  // Grab params off the param server
  private_nh_.param("use_map_topic", use_map_topic_, true);
  use_map_topic_=true;//modify by hualei
  private_nh_.param("first_map_only", first_map_only_, false);

  double tmp;
  private_nh_.param("gui_publish_rate", tmp, -1.0);
  gui_publish_period = ros::Duration(1.0/tmp);
  private_nh_.param("save_pose_rate", tmp, 0.5);
  save_pose_period = ros::Duration(1.0/tmp);

  private_nh_.param("laser_min_range", laser_min_range_, -1.0);
  private_nh_.param("laser_max_range", laser_max_range_, -1.0);
  private_nh_.param("laser_max_beams", max_beams_, 30);
  private_nh_.param("min_particles", min_particles_, 100);//100时精度５ＣＭ
  private_nh_.param("max_particles", max_particles_, 200);//default 5000
  private_nh_.param("kld_err", pf_err_, 0.01);
  private_nh_.param("kld_z", pf_z_, 0.99);
  private_nh_.param("odom_alpha1", alpha1_, 0.2);
  private_nh_.param("odom_alpha2", alpha2_, 0.2);
  private_nh_.param("odom_alpha3", alpha3_, 0.2);
  private_nh_.param("odom_alpha4", alpha4_, 0.2);
  private_nh_.param("odom_alpha5", alpha5_, 0.2);
  
  private_nh_.param("do_beamskip", do_beamskip_, false);
  private_nh_.param("beam_skip_distance", beam_skip_distance_, 0.5);
  private_nh_.param("beam_skip_threshold", beam_skip_threshold_, 0.3);
  private_nh_.param("beam_skip_error_threshold_", beam_skip_error_threshold_, 0.9);

  private_nh_.param("laser_z_hit", z_hit_, 0.95);
  private_nh_.param("laser_z_short", z_short_, 0.1);
  private_nh_.param("laser_z_max", z_max_, 0.05);
  private_nh_.param("laser_z_rand", z_rand_, 0.05);
  private_nh_.param("laser_sigma_hit", sigma_hit_, 0.2);
  private_nh_.param("laser_lambda_short", lambda_short_, 0.1);
  private_nh_.param("laser_likelihood_max_dist", laser_likelihood_max_dist_, 2.0);
  std::string tmp_model_type;
  private_nh_.param("laser_model_type", tmp_model_type, std::string("likelihood_field"));
  if(tmp_model_type == "beam")
    laser_model_type_ = LASER_MODEL_BEAM;
  else if(tmp_model_type == "likelihood_field")
    laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD;
  else if(tmp_model_type == "likelihood_field_prob"){
    laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD_PROB;
  }
  else
  {
    ROS_WARN("Unknown laser model type \"%s\"; defaulting to likelihood_field model",
             tmp_model_type.c_str());
    laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD;
  }

  private_nh_.param("odom_model_type", tmp_model_type, std::string("diff"));
  if(tmp_model_type == "diff")
    odom_model_type_ = ODOM_MODEL_DIFF;
  else if(tmp_model_type == "omni")
    odom_model_type_ = ODOM_MODEL_OMNI;
  else if(tmp_model_type == "diff-corrected")
    odom_model_type_ = ODOM_MODEL_DIFF_CORRECTED;
  else if(tmp_model_type == "omni-corrected")
    odom_model_type_ = ODOM_MODEL_OMNI_CORRECTED;
  else
  {
    ROS_WARN("Unknown odom model type \"%s\"; defaulting to diff model",
             tmp_model_type.c_str());
    odom_model_type_ = ODOM_MODEL_DIFF;
  }

  private_nh_.param("update_min_d", d_thresh_, 0.2);
  private_nh_.param("update_min_a", a_thresh_, M_PI/6.0);
  private_nh_.param("odom_frame_id", odom_frame_id_, std::string("odom"));
  private_nh_.param("base_frame_id", base_frame_id_, std::string("base_link"));
  private_nh_.param("global_frame_id", global_frame_id_, std::string("map"));
  private_nh_.param("resample_interval", resample_interval_, 2);
  double tmp_tol;
  private_nh_.param("transform_tolerance", tmp_tol, 0.1);//tf有效时间，单位ｓ.默认0.1,100.0可能会导致tf时间戳不匹配
  private_nh_.param("recovery_alpha_slow", alpha_slow_, 0.001);//解决粒子绑架问题
  private_nh_.param("recovery_alpha_fast", alpha_fast_, 0.001);//解决粒子绑架问题
  private_nh_.param("tf_broadcast", tf_broadcast_, true);


  transform_tolerance_.fromSec(tmp_tol);

  {
    double bag_scan_period;
    private_nh_.param("bag_scan_period", bag_scan_period, -1.0);
    bag_scan_period_.fromSec(bag_scan_period);
  }

  updatePoseFromServer();//初始化位置。

  resetParameter(configParameter);//hualei

  cloud_pub_interval.fromSec(1.0);
  tfb_ = new tf::TransformBroadcaster();
  tf_ = new TransformListenerWrapper();

  pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2, true);
//  mapMatchScore_pub_ =nh_.advertise<amcl::AmclExtension>("mapMatchScore", 10, true);//hualei
  socre_weight=0;//hualei
  for(int i=0;i<9;i++)
  {
      socre_pos_cov[i]=0;
  }
  particlecloud_pub_ = nh_.advertise<geometry_msgs::PoseArray>("particlecloud", 2, true);
  global_loc_srv_ = nh_.advertiseService("global_localization", 
					 &AmclNode::globalLocalizationCallback,
                                         this);
  nomotion_update_srv_= nh_.advertiseService("request_nomotion_update", &AmclNode::nomotionUpdateCallback, this);
  set_map_srv_= nh_.advertiseService("set_map", &AmclNode::setMapCallback, this);

  laser_scan_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh_, scan_topic_, 100);//hualei
  laser_scan_filter_ = 
          new tf::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_, 
                                                        *tf_, 
                                                        odom_frame_id_, 
                                                        100);
  laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
                                                   this, _1));
  initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);

  if(use_map_topic_) {
    map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);
    ROS_INFO("Subscribed to map topic.");
  } else {
    requestMap();
  }
  m_force_update = false;

  dsrv_ = new dynamic_reconfigure::Server<amcl::AMCLConfig>(ros::NodeHandle("~"));
  dynamic_reconfigure::Server<amcl::AMCLConfig>::CallbackType cb = boost::bind(&AmclNode::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);

  // D_LASER_LOSS_MAX_TIME timer to warn on lack of receipt of laser scans, #5209
  laser_check_interval_ = ros::Duration(D_LASER_LOSS_MAX_TIME);
  check_laser_timer_ = nh_.createTimer(laser_check_interval_, 
                                       boost::bind(&AmclNode::checkLaserReceived, this, _1));
}

void AmclNode::reconfigureCB(AMCLConfig &config, uint32_t level)
{
  boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);

  //we don't want to do anything on the first call
  //which corresponds to startup
  if(first_reconfigure_call_)
  {
    first_reconfigure_call_ = false;
    default_config_ = config;
    return;
  }

  if(config.restore_defaults) {
    config = default_config_;
    //avoid looping
    config.restore_defaults = false;
  }

  d_thresh_ = config.update_min_d;
  a_thresh_ = config.update_min_a;

  resample_interval_ = config.resample_interval;

  laser_min_range_ = config.laser_min_range;
  laser_max_range_ = config.laser_max_range;

  gui_publish_period = ros::Duration(1.0/config.gui_publish_rate);
  save_pose_period = ros::Duration(1.0/config.save_pose_rate);

  transform_tolerance_.fromSec(config.transform_tolerance);

  max_beams_ = config.laser_max_beams;
  alpha1_ = config.odom_alpha1;
  alpha2_ = config.odom_alpha2;
  alpha3_ = config.odom_alpha3;
  alpha4_ = config.odom_alpha4;
  alpha5_ = config.odom_alpha5;

  z_hit_ = config.laser_z_hit;
  z_short_ = config.laser_z_short;
  z_max_ = config.laser_z_max;
  z_rand_ = config.laser_z_rand;
  sigma_hit_ = config.laser_sigma_hit;
  lambda_short_ = config.laser_lambda_short;
  laser_likelihood_max_dist_ = config.laser_likelihood_max_dist;

  if(config.laser_model_type == "beam")
    laser_model_type_ = LASER_MODEL_BEAM;
  else if(config.laser_model_type == "likelihood_field")
    laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD;
  else if(config.laser_model_type == "likelihood_field_prob")
    laser_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD_PROB;

  if(config.odom_model_type == "diff")
    odom_model_type_ = ODOM_MODEL_DIFF;
  else if(config.odom_model_type == "omni")
    odom_model_type_ = ODOM_MODEL_OMNI;
  else if(config.odom_model_type == "diff-corrected")
    odom_model_type_ = ODOM_MODEL_DIFF_CORRECTED;
  else if(config.odom_model_type == "omni-corrected")
    odom_model_type_ = ODOM_MODEL_OMNI_CORRECTED;

  if(config.min_particles > config.max_particles)
  {
    ROS_WARN("You've set min_particles to be greater than max particles, this isn't allowed so they'll be set to be equal.");
    config.max_particles = config.min_particles;
  }

  min_particles_ = config.min_particles;
  max_particles_ = config.max_particles;
  alpha_slow_ = config.recovery_alpha_slow;
  alpha_fast_ = config.recovery_alpha_fast;
  tf_broadcast_ = config.tf_broadcast;

  do_beamskip_= config.do_beamskip; 
  beam_skip_distance_ = config.beam_skip_distance; 
  beam_skip_threshold_ = config.beam_skip_threshold; 

  pf_ = pf_alloc(min_particles_, max_particles_,
                 alpha_slow_, alpha_fast_,
                 (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
                 (void *)map_);
  pf_err_ = config.kld_err; 
  pf_z_ = config.kld_z; 
  pf_->pop_err = pf_err_;
  pf_->pop_z = pf_z_;

  // Initialize the filter
  pf_vector_t pf_init_pose_mean = pf_vector_zero();
  pf_init_pose_mean.v[0] = last_published_pose.pose.pose.position.x;
  pf_init_pose_mean.v[1] = last_published_pose.pose.pose.position.y;
  pf_init_pose_mean.v[2] = tf::getYaw(last_published_pose.pose.pose.orientation);
  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
  pf_init_pose_cov.m[0][0] = last_published_pose.pose.covariance[6*0+0];
  pf_init_pose_cov.m[1][1] = last_published_pose.pose.covariance[6*1+1];
  pf_init_pose_cov.m[2][2] = last_published_pose.pose.covariance[6*5+5];
  pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
  pf_init_ = false;

  // Instantiate the sensor objects
  // Odometry
  delete odom_;
  odom_ = new AMCLOdom();
  ROS_ASSERT(odom_);
  odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
  // Laser
  delete laser_;
  laser_ = new AMCLLaser(max_beams_, map_);
  ROS_ASSERT(laser_);
  if(laser_model_type_ == LASER_MODEL_BEAM)
    laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,
                         sigma_hit_, lambda_short_, 0.0);
  else if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD_PROB){
    ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
    laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_,
					laser_likelihood_max_dist_, 
					do_beamskip_, beam_skip_distance_, 
					beam_skip_threshold_, beam_skip_error_threshold_);
    ROS_INFO("Done initializing likelihood field model with probabilities.");
  }
  else if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD){
    ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
    laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
                                    laser_likelihood_max_dist_);
    ROS_INFO("Done initializing likelihood field model.");
  }

  odom_frame_id_ = config.odom_frame_id;
  base_frame_id_ = config.base_frame_id;
  global_frame_id_ = config.global_frame_id;

  delete laser_scan_filter_;
  laser_scan_filter_ = 
          new tf::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_, 
                                                        *tf_, 
                                                        odom_frame_id_, 
                                                        100);
  laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,
                                                   this, _1));

  initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);
}


void AmclNode::runFromBag(const std::string &in_bag_fn)
{
  rosbag::Bag bag;
  bag.open(in_bag_fn, rosbag::bagmode::Read);
  std::vector<std::string> topics;
  topics.push_back(std::string("tf"));
  std::string scan_topic_name = "base_scan"; // TODO determine what topic this actually is from ROS
  topics.push_back(scan_topic_name);
  rosbag::View view(bag, rosbag::TopicQuery(topics));

  ros::Publisher laser_pub = nh_.advertise<sensor_msgs::LaserScan>(scan_topic_name, 100);
  ros::Publisher tf_pub = nh_.advertise<tf2_msgs::TFMessage>("/tf", 100);

  // Sleep for a second to let all subscribers connect
  ros::WallDuration(1.0).sleep();

  ros::WallTime start(ros::WallTime::now());

  // Wait for map
  while (ros::ok())
  {
    {
      boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
      if (map_)
      {
        ROS_INFO("Map is ready");
        break;
      }
    }
    ROS_INFO("Waiting for map...");
    ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(1.0));
  }

  BOOST_FOREACH(rosbag::MessageInstance const msg, view)
  {
    if (!ros::ok())
    {
      break;
    }

    // Process any ros messages or callbacks at this point
    ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration());

    tf2_msgs::TFMessage::ConstPtr tf_msg = msg.instantiate<tf2_msgs::TFMessage>();
    if (tf_msg != NULL)
    {
      tf_pub.publish(msg);
      for (size_t ii=0; ii<tf_msg->transforms.size(); ++ii)
      {
        tf_->getBuffer().setTransform(tf_msg->transforms[ii], "rosbag_authority");
      }
      continue;
    }

    sensor_msgs::LaserScan::ConstPtr base_scan = msg.instantiate<sensor_msgs::LaserScan>();
    if (base_scan != NULL)
    {
      laser_pub.publish(msg);
      laser_scan_filter_->add(base_scan);
      if (bag_scan_period_ > ros::WallDuration(0))
      {
        bag_scan_period_.sleep();
      }
      continue;
    }

    ROS_WARN_STREAM("Unsupported message type" << msg.getTopic());
  }

  bag.close();

  double runtime = (ros::WallTime::now() - start).toSec();
  ROS_INFO("Bag complete, took %.1f seconds to process, shutting down", runtime);

  const geometry_msgs::Quaternion & q(last_published_pose.pose.pose.orientation);
  double yaw, pitch, roll;
  tf::Matrix3x3(tf::Quaternion(q.x, q.y, q.z, q.w)).getEulerYPR(yaw,pitch,roll);
  ROS_INFO("Final location %.3f, %.3f, %.3f with stamp=%f",
            last_published_pose.pose.pose.position.x,
            last_published_pose.pose.pose.position.y,
            yaw, last_published_pose.header.stamp.toSec()
            );

  ros::shutdown();
}


void AmclNode::savePoseToServer()
{
  // We need to apply the last transform to the latest odom pose to get
  // the latest map pose to store.  We'll take the covariance from
  // last_published_pose.
  tf::Pose map_pose = latest_tf_.inverse() * latest_odom_pose_;
  double yaw,pitch,roll;
  map_pose.getBasis().getEulerYPR(yaw, pitch, roll);

  ROS_DEBUG("Saving pose to server. x: %.3f, y: %.3f", map_pose.getOrigin().x(), map_pose.getOrigin().y() );

  private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x());
  private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y());
  private_nh_.setParam("initial_pose_a", yaw);
  private_nh_.setParam("initial_cov_xx", 
                                  last_published_pose.pose.covariance[6*0+0]);
  private_nh_.setParam("initial_cov_yy", 
                                  last_published_pose.pose.covariance[6*1+1]);
  private_nh_.setParam("initial_cov_aa", 
                       last_published_pose.pose.covariance[6*5+5]);
}

bool AmclNode::isLaserReceiveOkStatus()
{
    return isLaserReceiveOk;
}

bool AmclNode::isPfOkStatus()
{
    return isPfOk;
}

int AmclNode::setPartialNum(int minNum, int maxNum,int maxBeams)
{
    boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
    min_particles_ = minNum;
    max_particles_ = maxNum;
    maxBeams=maxBeams;
    return 1;
}



int AmclNode::setDeltaDistanceUpdate(double deltaDistance)
{
//    boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);//导致没有工作了吗？
    d_thresh_ = deltaDistance;
    return 1;
}

int AmclNode::setDeltaAngleUpdate(double deltaAngle)
{
//    boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);//导致没有工作了吗？
    a_thresh_ = deltaAngle;
    return 1;
}

void AmclNode::sendOdomToMapTf(VehiclePosition positionIn)
{
    boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
    // subtracting base to odom from map to base and send map to odom instead
    tf::Stamped<tf::Pose> odom_to_map;
    try
    {
      tf::Transform tmp_tf(tf::createQuaternionFromYaw(positionIn.rotate),
                           tf::Vector3(positionIn.x,
                                       positionIn.y,
                                       0.0));
      tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
                                            ros::Time::now(),
                                            base_frame_id_);
      this->tf_->transformPose(odom_frame_id_,
                               tmp_tf_stamped,
                               odom_to_map);
    }
    catch(tf::TransformException)
    {
      qDebug()<<"Failed to subtract base to odom transform222";
      return ;
    }

    latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
                               tf::Point(odom_to_map.getOrigin()));
    latest_tf_valid_ = true;

    if (tf_broadcast_ == true)
    {
      // We want to send a transform that is good up until a
      // tolerance time so that odom can be used
      ros::Time transform_expiration = (ros::Time::now() +
                                        transform_tolerance_);
      tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
                                          transform_expiration,
                                          global_frame_id_, odom_frame_id_);
      this->tfb_->sendTransform(tmp_tf_stamped);
      sent_first_transform_ = true;

    }
}

int AmclNode::calculateCorrectPosition(PositionInfo reflectPositionIn, double &xOut, double &yOut, double &rOut)
{
    if(reflectPositionIn.isUpdateSucess )
    {

        double tmpDistance=sqrt((reflectPositionIn.position.x-xOut)*
                                (reflectPositionIn.position.x-xOut)+
                                (reflectPositionIn.position.y-yOut)*
                                (reflectPositionIn.position.y-yOut));
        double tmpAngle=angleRotationJudgement(reflectPositionIn.position.rotate,rOut);
        if(tmpDistance<correctAllowMaxDistanceOffset &&fabs(tmpAngle)<correctAllowMaxAngleOffset
                && qualifyReflectMinCertaintyScore<reflectPositionIn.matchScore)
        {
            debugInfo.correctPositionOffset.x=reflectPositionIn.position.x-xOut;
            debugInfo.correctPositionOffset.y=reflectPositionIn.position.y-yOut;
            debugInfo.correctPositionOffset.rotate=tmpAngle;
            debugInfo.correctCount++;
            debugInfo.isCorrectSucess=1;

            //做平滑过渡. 已经有了里程计平滑，最终位置平滑．
            xOut+=debugInfo.correctPositionOffset.x/correctFilterCount;
            yOut+=debugInfo.correctPositionOffset.y/correctFilterCount;
            rOut+=tmpAngle/correctFilterCount;


            return 1;
        }
        else
        {
            debugInfo.isCorrectSucess=-1;
            qDebug()<<"AmclNode::calculateCorrectPosition:: bias to big,tmpDistance"<<tmpDistance<<"tmpAngle"<<tmpAngle;
            return -1;
        }

    }
    debugInfo.isCorrectSucess=0;
    return 0;
}


double AmclNode::angleRotationJudgement(double targetAngle, double initialAngle)
{
    double angle1,angle2,angle3;
    angle1=targetAngle-initialAngle;
    angle2=angle1+2*M_PI;
    angle3=angle1-2*M_PI;
    double minNum;
    if(fabs(angle1)<=fabs(angle2))
    {
        minNum=angle1;
    }
    else
    {
        minNum=angle2;
    }

    if(fabs(angle3)<=fabs(minNum))
    {
        minNum=angle3;
    }

    return minNum;

}



int AmclNode::checkMatchScoreAndChangeOdom()
{
    static int  odomMode=0;

    if(positionCertaintyFactor<badMatchScore && 0!=odomMode)
    {
        qDebug()<<"checkMatchScoreAndChangeOdom use bad odom,positionCertaintyFactor"<<positionCertaintyFactor;
        boost::recursive_mutex::scoped_lock cfl1(configuration_mutex_);
        alpha1_=newOdom_alpha_badMatchScore;
        alpha2_=newOdom_alpha_badMatchScore;
        alpha3_=newOdom_alpha_badMatchScore;
        alpha4_=newOdom_alpha_badMatchScore;
        alpha5_=newOdom_alpha_badMatchScore;
        delete odom_;
        odom_ = new AMCLOdom();
        ROS_ASSERT(odom_);
        odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
        odomMode=0;
    }
    else if(positionCertaintyFactor>badMatchScore && 1!=odomMode)
    {
        qDebug()<<"checkMatchScoreAndChangeOdom use good odom,positionCertaintyFactor"<<positionCertaintyFactor;
        boost::recursive_mutex::scoped_lock cfl2(configuration_mutex_);
        alpha1_=configParameter->odom_alpha1;
        alpha2_=configParameter->odom_alpha2;
        alpha3_=configParameter->odom_alpha3;
        alpha4_=configParameter->odom_alpha4;
        alpha5_=configParameter->odom_alpha5;
        delete odom_;
        odom_ = new AMCLOdom();
        ROS_ASSERT(odom_);
        odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
        odomMode=1;
    }
    return 1;
}

int AmclNode::resetParameter(AmclConfig* configParameterIn)
{
    max_beams_=configParameterIn->laser_max_beams;
    laser_model_type_=configParameterIn->laser_model_type;
    max_particles_=configParameterIn->max_particles;
    min_particles_=configParameterIn->min_particles;
    odom_model_type_=configParameterIn->odom_model_type;
    a_thresh_=configParameterIn->update_min_a;
    d_thresh_=configParameterIn->update_min_d;
    alpha1_=configParameterIn->odom_alpha1;
    alpha2_=configParameterIn->odom_alpha2;
    alpha3_=configParameterIn->odom_alpha3;
    alpha4_=configParameterIn->odom_alpha4;
    alpha5_=configParameterIn->odom_alpha5;
    isUpdatePartialEveryTime=configParameterIn->isUpdatePartialEveryTime;
    calculateTimesForLaser=configParameterIn->calculateTimesForLaser;
//    isUseRosLaserMsg=configParameterIn->isUseRosLaserMsg;
//    isUseRosLaserMsg=true;
    goodMatchScore=configParameterIn->badMatchScore;
    badMatchScore=configParameterIn->badMatchScore;
    newOdom_alpha_badMatchScore=configParameterIn->newOdom_alpha_badMatchScore;
    init_pose_[0] = configParameterIn->initial_pose_x;
    init_pose_[1] = configParameterIn->initial_pose_y;
    init_pose_[2] = configParameterIn->initial_pose_a;
    qDebug()<<"AmclNode::resetParameter init_pose_[0]"<<init_pose_[0]<<init_pose_[1]<<init_pose_[2];
//    init_cov_[0] = D_INITIAL_POSE_COV;//0.25有点过大了．
//    init_cov_[1] = D_INITIAL_POSE_COV;//0.25有点过大了．
//    init_cov_[2] = (M_PI/12.0) * (M_PI/12.0);
    init_cov_[0] = configParameterIn->initial_cov_x;
    init_cov_[1] = configParameterIn->initial_cov_y;
    init_cov_[2] = configParameterIn->initial_cov_r;

    alpha_slow_=configParameterIn->recovery_alpha_slow;
    alpha_fast_=configParameterIn->recovery_alpha_fast;
    resample_interval_=configParameterIn->resample_interval;
//    resample_interval_=calculateTimesForLaser;
    odomOffsetFilterCount=configParameterIn->odomOffsetFilterCount;

    isCorrectByReflect=configParameterIn->isCorrectByReflect;
    qualifyReflectMinCertaintyScore=configParameterIn->qualifyReflectMinCertaintyScore;
    correctFilterCount=configParameterIn->correctFilterCount;
    correctAllowMaxDistanceOffset=configParameterIn->correctAllowMaxDistanceOffset;
    correctAllowMaxAngleOffset=configParameterIn->correctAllowMaxAngleOffset;
    return 1;
}

int AmclNode::laserDataCalculate(const sensor_msgs::LaserScanConstPtr &laser_scan)
{

//    qDebug()<<"AmclNode::laserDataCalculate";
    if(false==isLaserReadyForCalculate)
    {
//        qDebug()<<"AmclNode::laserDataCalculate no laserdata ready";
        return 0;//可能是激光数据没有准备好，导致失败吗？
    }
    isLaserReadyForCalculate=false;
    laserRequestCount=0;//激光数据正常复位
    //时间分析
    struct timespec startTime,endTime;
    clock_gettime(CLOCK_MONOTONIC, &startTime);

    if( map_ == NULL ) {
      return -1;
    }
    boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
    int laser_index = -1;

    // Do we have the base->base_laser Tx yet?
    if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end())
    {
      ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan->header.frame_id.c_str());
      lasers_.push_back(new AMCLLaser(*laser_));
      lasers_update_.push_back(true);
      laser_index = frame_to_laser_.size();

      tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
                                               tf::Vector3(0,0,0)),
                                   ros::Time(), laser_scan->header.frame_id);
      tf::Stamped<tf::Pose> laser_pose;
      try
      {
        this->tf_->transformPose(base_frame_id_, ident, laser_pose);
      }
      catch(tf::TransformException& e)
      {
        ROS_ERROR("Couldn't transform from %s to %s, "
                  "even though the message notifier is in use",
                  laser_scan->header.frame_id.c_str(),
                  base_frame_id_.c_str());
        return -2;
      }

      pf_vector_t laser_pose_v;
      laser_pose_v.v[0] = laser_pose.getOrigin().x();
      laser_pose_v.v[1] = laser_pose.getOrigin().y();
      // laser mounting angle gets computed later -> set to 0 here!
      laser_pose_v.v[2] = 0;
      lasers_[laser_index]->SetLaserPose(laser_pose_v);
      ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
                laser_pose_v.v[0],
                laser_pose_v.v[1],
                laser_pose_v.v[2]);

      frame_to_laser_[laser_scan->header.frame_id] = laser_index;
    } else {
      // we have the laser pose, retrieve laser index
      laser_index = frame_to_laser_[laser_scan->header.frame_id];
    }

    // Where was the robot when this scan was taken?
    pf_vector_t pose;//需要保证同一帧激光数据的多次计算里程计一直不变。
    if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
                    laser_scan->header.stamp, base_frame_id_))
    {
      ROS_ERROR("amcl Couldn't determine robot's pose associated with laser scan");
      return -3;
    }


    pf_vector_t delta = pf_vector_zero();

    if(pf_init_)
    {
      // Compute change in pose
      //delta = pf_vector_coord_sub(pose, pf_odom_pose_);
      delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
      delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
      delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);

      // See if we should update the filter
      bool update = fabs(delta.v[0]) > d_thresh_ ||
                    fabs(delta.v[1]) > d_thresh_ ||
                    fabs(delta.v[2]) > a_thresh_;
      //hualei 每多少次更新一次
      static int tmp_count=0;
      bool tmp_isUpdate=false;
      if(isUpdatePartialEveryTime>0)
      {
          tmp_count++;
          if(tmp_count==isUpdatePartialEveryTime)
          {
              tmp_isUpdate=true;
              tmp_count=0;
          }
      }
      update = update || m_force_update || tmp_isUpdate;//hualei
      m_force_update=false;

      // Set the laser update flags
      if(update)
        for(unsigned int i=0; i < lasers_update_.size(); i++)
          lasers_update_[i] = true;
    }

    bool force_publication = false;
    if(!pf_init_)
    {
      // Pose at last filter update
      pf_odom_pose_ = pose;

      // Filter is now initialized
      pf_init_ = true;

      // Should update sensor data
      for(unsigned int i=0; i < lasers_update_.size(); i++)
        lasers_update_[i] = true;

      force_publication = true;

      resample_count_ = 0;
    }
    // If the robot has moved, update the filter
    else if(pf_init_ && lasers_update_[laser_index])
    {
      //printf("pose\n");
      //pf_vector_fprintf(pose, stdout, "%.3f");

      AMCLOdomData odata;
      odata.pose = pose;
      // HACK
      // Modify the delta in the action data so the filter gets
      // updated correctly
      odata.delta = delta;

      // Use the action data to update the filter
      odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);

      // Pose at last filter update
      //this->pf_odom_pose = pose;
    }

    bool resampled = false;
    // If the robot has moved, update the filter
    if(lasers_update_[laser_index])
    {
      AMCLLaserData ldata;
      ldata.sensor = lasers_[laser_index];
      ldata.range_count = laser_scan->ranges.size();

      // To account for lasers that are mounted upside-down, we determine the
      // min, max, and increment angles of the laser in the base frame.
      //
      // Construct min and max angles of laser, in the base_link frame.
      tf::Quaternion q;
      q.setRPY(0.0, 0.0, laser_scan->angle_min);
      tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
                                        laser_scan->header.frame_id);
      q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
      tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
                                        laser_scan->header.frame_id);
      try
      {
        tf_->transformQuaternion(base_frame_id_, min_q, min_q);
        tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
      }
      catch(tf::TransformException& e)
      {
        ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
                 e.what());
        return -4;
      }

      double angle_min = tf::getYaw(min_q);
      double angle_increment = tf::getYaw(inc_q) - angle_min;

      // wrapping angle to [-pi .. pi]
      angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;

      ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);

      // Apply range min/max thresholds, if the user supplied them
      if(laser_max_range_ > 0.0)
        ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
      else
        ldata.range_max = laser_scan->range_max;
      double range_min;
      if(laser_min_range_ > 0.0)
        range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
      else
        range_min = laser_scan->range_min;
      // The AMCLLaserData destructor will free this memory
      ldata.ranges = new double[ldata.range_count][2];
      ROS_ASSERT(ldata.ranges);
      for(int i=0;i<ldata.range_count;i++)
      {
        // amcl doesn't (yet) have a concept of min range.  So we'll map short
        // readings to max range.
        if(laser_scan->ranges[i] <= range_min)
          ldata.ranges[i][0] = ldata.range_max;
        else
          ldata.ranges[i][0] = laser_scan->ranges[i];
        // Compute bearing
        ldata.ranges[i][1] = angle_min +
                (i * angle_increment);
      }

      lasers_[laser_index]->UpdateSensor(&positionCertaintyFactor,pf_, (AMCLSensorData*)&ldata);
  //    positionCertaintyFactor=lasers_[laser_index]->calculateDistanceBetweenLaserAndMap((AMCLLaserData*)&ldata,current_pose_mean);//hualei 计算地图偏差

      lasers_update_[laser_index] = false;

      pf_odom_pose_ = pose;

      // Resample the particles
      if(!(++resample_count_ % resample_interval_))
      {
        pf_update_resample(pf_);
        resampled = true;
      }

      pf_sample_set_t* set = pf_->sets + pf_->current_set;
      ROS_DEBUG("Num samples: %d\n", set->sample_count);

      // Publish the resulting cloud
      // TODO: set maximum rate for publishing
      if (!m_force_update) {
        geometry_msgs::PoseArray cloud_msg;
        cloud_msg.header.stamp = ros::Time::now();
        cloud_msg.header.frame_id = global_frame_id_;
        cloud_msg.poses.resize(set->sample_count);
        for(int i=0;i<set->sample_count;i++)
        {
          tf::poseTFToMsg(tf::Pose(tf::createQuaternionFromYaw(set->samples[i].pose.v[2]),
                                   tf::Vector3(set->samples[i].pose.v[0],
                                             set->samples[i].pose.v[1], 0)),
                          cloud_msg.poses[i]);
        }
        particlecloud_pub_.publish(cloud_msg);
      }
    }

    if(resampled || force_publication)
    {
      if (!resampled)
      {
          // re-compute the cluster statistics
          int tmpReturn = pf_cluster_stats(pf_, pf_->sets);
          if(1!=tmpReturn)//hualei
          {
              isPfOk=false;
              return -5;
          }
          else
          {
              isPfOk=true;
          }
      }
      // Read out the current hypotheses
      double max_weight = 0.0;
      int max_weight_hyp = -1;
      std::vector<amcl_hyp_t> hyps;
      hyps.resize(pf_->sets[pf_->current_set].cluster_count);
      for(int hyp_count = 0;
          hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
      {
        double weight;
        pf_vector_t pose_mean;
        pf_matrix_t pose_cov;
        if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
        {
          ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
          break;
        }

        hyps[hyp_count].weight = weight;
        hyps[hyp_count].pf_pose_mean = pose_mean;
        hyps[hyp_count].pf_pose_cov = pose_cov;
        current_pose_mean= pose_mean;//hualei

        if(hyps[hyp_count].weight > max_weight)
        {
          max_weight = hyps[hyp_count].weight;
          max_weight_hyp = hyp_count;
        }
      }

      if(max_weight > 0.0)
      {
        socre_weight=max_weight;//hualei
        socre_pos_cov[0]=hyps[max_weight_hyp].pf_pose_cov.m[0][0] ;//hualei
        socre_pos_cov[1]=hyps[max_weight_hyp].pf_pose_cov.m[0][1] ;//hualei
        socre_pos_cov[2]=hyps[max_weight_hyp].pf_pose_cov.m[0][2] ;//hualei
        socre_pos_cov[3]=hyps[max_weight_hyp].pf_pose_cov.m[1][0] ;//hualei
        socre_pos_cov[4]=hyps[max_weight_hyp].pf_pose_cov.m[1][1] ;//hualei
        socre_pos_cov[5]=hyps[max_weight_hyp].pf_pose_cov.m[1][2] ;//hualei
        socre_pos_cov[6]=hyps[max_weight_hyp].pf_pose_cov.m[2][0] ;//hualei
        socre_pos_cov[7]=hyps[max_weight_hyp].pf_pose_cov.m[2][1] ;//hualei
        socre_pos_cov[8]=hyps[max_weight_hyp].pf_pose_cov.m[2][3] ;//hualei
        ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
                  hyps[max_weight_hyp].pf_pose_mean.v[0],
                  hyps[max_weight_hyp].pf_pose_mean.v[1],
                  hyps[max_weight_hyp].pf_pose_mean.v[2]);

        /*
           puts("");
           pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
           puts("");
         */

        geometry_msgs::PoseWithCovarianceStamped p;
        // Fill in the header
        p.header.frame_id = global_frame_id_;
        p.header.stamp = laser_scan->header.stamp;
        // Copy in the pose
        p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
        p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
        tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                              p.pose.pose.orientation);
        // Copy in the covariance, converting from 3-D to 6-D
        pf_sample_set_t* set = pf_->sets + pf_->current_set;
        for(int i=0; i<2; i++)
        {
          for(int j=0; j<2; j++)
          {
            // Report the overall filter covariance, rather than the
            // covariance for the highest-weight cluster
            //p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
            p.pose.covariance[6*i+j] = set->cov.m[i][j];
          }
        }
        // Report the overall filter covariance, rather than the
        // covariance for the highest-weight cluster
        //p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
        p.pose.covariance[6*5+5] = set->cov.m[2][2];

        /*
           printf("cov:\n");
           for(int i=0; i<6; i++)
           {
           for(int j=0; j<6; j++)
           printf("%6.3f ", p.covariance[6*i+j]);
           puts("");
           }
         */

        pose_pub_.publish(p);
        last_published_pose = p;

        ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
                 hyps[max_weight_hyp].pf_pose_mean.v[0],
                 hyps[max_weight_hyp].pf_pose_mean.v[1],
                 hyps[max_weight_hyp].pf_pose_mean.v[2]);

        // subtracting base to odom from map to base and send map to odom instead
        tf::Stamped<tf::Pose> odom_to_map;
        try
        {
          tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                               tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
                                           hyps[max_weight_hyp].pf_pose_mean.v[1],
                                           0.0));
          tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
                                                laser_scan->header.stamp,
                                                base_frame_id_);
          this->tf_->transformPose(odom_frame_id_,
                                   tmp_tf_stamped,
                                   odom_to_map);
        }
        catch(tf::TransformException)
        {
          ROS_DEBUG("Failed to subtract base to odom transform");
          return -6;
        }

        latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
                                   tf::Point(odom_to_map.getOrigin()));
        latest_tf_valid_ = true;

        if (tf_broadcast_ == true)
        {
          // We want to send a transform that is good up until a
          // tolerance time so that odom can be used
          ros::Time transform_expiration = (laser_scan->header.stamp +
                                            transform_tolerance_);
          tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
                                              transform_expiration,
                                              global_frame_id_, odom_frame_id_);
          this->tfb_->sendTransform(tmp_tf_stamped);
          sent_first_transform_ = true;
  //        mapMatchScore_pub_.publish(mathWightToPub);//modify by hualei
        }
      }
      else
      {
        ROS_ERROR("No pose!");
      }
    }
    else if(latest_tf_valid_)
    {
      if (tf_broadcast_ == true)
      {
        // Nothing changed, so we'll just republish the last transform, to keep
        // everybody happy.
        ros::Time transform_expiration = (laser_scan->header.stamp +
                                          transform_tolerance_);
        tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
                                            transform_expiration,
                                            global_frame_id_, odom_frame_id_);
        this->tfb_->sendTransform(tmp_tf_stamped);
  //      mapMatchScore_pub_.publish(mathWightToPub);//modify by hualei
      }

      // Is it time to save our last pose to the param server
      ros::Time now = ros::Time::now();
      if((save_pose_period.toSec() > 0.0) &&
         (now - save_pose_last_time) >= save_pose_period)
      {
        this->savePoseToServer();
        save_pose_last_time = now;
      }
    }

    extRequestCount=0;//复位看门狗计数
    clock_gettime(CLOCK_MONOTONIC, &endTime);
    currentConsumed=1000000000*(endTime.tv_sec-startTime.tv_sec)+endTime.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(currentConsumed<minConsumed)
    {
        minConsumed=currentConsumed;
    }
    if(currentConsumed>maxConsumed)
    {
        maxConsumed=currentConsumed;
    }
    return 1;
}
int AmclNode::laserDataCalculate_multicalculate(const sensor_msgs::LaserScanConstPtr &laser_scan,bool& isReSetCloudOut)
{

    if(false==isCalculateLaser)
    {
        qDebug()<<"AmclNode::laserDataCalculate false==isCalculateLaser";
        return 0;//可能是激光数据没有准备好，导致失败吗？
    }
    //里程计不动时或低速微动，如果原版的算法，一直迭代计算，会在一面墙的情况时，位置蠕动，导致失真。
    //如果只计算一次，导致迭代的精度不够。
    //里程计变化超过阈值时，才开始计算，而且计算的次数要限制。或者要限制粒子重新分布的范围不得超过当前里程计的误差范围。

//    qDebug()<<"AmclNode::laserDataCalculate";
    if(false==isLaserReadyForCalculate)
    {
        if(debugFlag)
        {
            qDebug()<<"AmclNode::laserDataCalculate no laserdata ready";
        }

        return 0;//可能是激光数据没有准备好，导致失败吗？
    }
//    checkMatchScoreAndChangeOdom();//设置合理的绑架系数就可以了。必须设置.
    isLaserReadyForCalculate=false;
    laserRequestCount=0;//激光数据正常复位
    //时间分析
    struct timespec startTime,endTime;
    clock_gettime(CLOCK_MONOTONIC, &startTime);


    //保证连续多次计算的激光数据不变，里程计不变。todo tf数据可能变了？
//    qDebug()<<"000 laserDataCalculate_multicalculate";
    int calculateResult=0;
    sensor_msgs::LaserScanConstPtr laser_scan_lock=laser_scan;//被智能指针引用后，内部加锁了。
//    for(int i=0;i<calculateTimesForLaser;i++)
//    {
//        if(1==laserDataCalculate_multicalculate_once(laser_scan_lock,i))
//        {
//            calculateResult++;
//        }
//    }

    //仅计算1次
//    calculateResult=laserDataCalculate_multicalculate_once(laser_scan_lock,0);

    calculateResult=laserDataCalculate_internalICP(laser_scan_lock,calculateTimesForLaser,isReSetCloudOut);

    //没有收到激光数据也要发送tf，否则tf停止了。已经发送过，才可以发送旧的。
    // We want to send a transform that is good up until a
    // tolerance time so that odom can be used
    if(calculateResult<=0)//太耗时间,不发布rviz显示不正常
    {
        ros::Time transform_expiration = (ros::Time::now() +transform_tolerance_);
        tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
                                            transform_expiration,
                                            global_frame_id_, odom_frame_id_);
        this->tfb_->sendTransform(tmp_tf_stamped);
    }

    extRequestCount=0;//复位看门狗计数
    clock_gettime(CLOCK_MONOTONIC, &endTime);

    currentConsumed=1000000000*(endTime.tv_sec-startTime.tv_sec)+endTime.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(currentConsumed<minConsumed)
    {
        minConsumed=currentConsumed;
    }
    if(currentConsumed>maxConsumed)
    {
        maxConsumed=currentConsumed;
    }


    if(debugFlag)
    {
        qDebug()<<"amcl time:currentConsumed"<<currentConsumed<<"calculateResult"<<calculateResult;
    }

    if(calculateResult>=1)
    {
        return 1;
    }
    return 0;
}
int AmclNode::laserDataCalculate_multicalculate_once(const sensor_msgs::LaserScanConstPtr &laser_scan,
                                                     int calculateCount)
{
    qDebug()<<"AmclNode::laserDataCalculate_multicalculate_once calculateCount"<<calculateCount;
    if(debugFlag)
    {
        qDebug()<<"AmclNode::laserDataCalculate_multicalculate_once";
    }
    //时间分析
    struct timespec startTime,endTime7,endTime2,endTime3,endTime4,endTime5,endTime6
            ,endTime5a,endTime5b,endTime5c;
    clock_gettime(CLOCK_MONOTONIC, &startTime);
    double currentConsumed;


    bool isCalculateSucess=false;

    if( map_ == NULL )
    {
        if(1)
        {
            qDebug()<<"amcl failed1,map_ == NULL";
        }
      return -1;
    }
    boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
    int laser_index = -1;

    // Do we have the base->base_laser Tx yet?
    if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end())
    {
      ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan->header.frame_id.c_str());
      lasers_.push_back(new AMCLLaser(*laser_));
      lasers_update_.push_back(true);
      laser_index = frame_to_laser_.size();

      tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
                                               tf::Vector3(0,0,0)),
                                   ros::Time(), laser_scan->header.frame_id);
      tf::Stamped<tf::Pose> laser_pose;
      try
      {
        this->tf_->transformPose(base_frame_id_, ident, laser_pose);
      }
      catch(tf::TransformException& e)
      {
        ROS_ERROR("Couldn't transform from %s to %s, "
                  "even though the message notifier is in use",
                  laser_scan->header.frame_id.c_str(),
                  base_frame_id_.c_str());
        if(1)
        {
            qDebug()<<"amcl failed2,TransformException";
        }
        return -2;
      }

      static pf_vector_t laser_pose_v;
      if(0==calculateCount)
      {
          laser_pose_v.v[0] = laser_pose.getOrigin().x();
          laser_pose_v.v[1] = laser_pose.getOrigin().y();
          // laser mounting angle gets computed later -> set to 0 here!
          laser_pose_v.v[2] = 0;
      }

      lasers_[laser_index]->SetLaserPose(laser_pose_v);
      ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
                laser_pose_v.v[0],
                laser_pose_v.v[1],
                laser_pose_v.v[2]);

      frame_to_laser_[laser_scan->header.frame_id] = laser_index;
    } else {
      // we have the laser pose, retrieve laser index
      laser_index = frame_to_laser_[laser_scan->header.frame_id];
    }

//    clock_gettime(CLOCK_MONOTONIC, &endTime1);
//    currentConsumed=1000000000*(endTime1.tv_sec-startTime.tv_sec)+endTime1.tv_nsec-startTime.tv_nsec;
//    currentConsumed=currentConsumed/1000.0;
//    if(debugFlag)
//    {
//        qDebug()<<"amcl endTime1:"<<currentConsumed;
//    }

    // Where was the robot when this scan was taken?
    static pf_vector_t pose;//需要保证同一帧激光数据的多次计算里程计一直不变。
    if(0==calculateCount)
    {
        //可能导致ros位置失败．
        if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
                        laser_scan->header.stamp, base_frame_id_))
        {
//          ROS_ERROR("amcl Couldn't determine robot's pose associated with laser scan");
//          if(debugFlag)
//          {
            if(initialAgvPositionCount>0)
            {
                initialAgvPositionCount--;
            }
            qDebug()<<"amcl failed3,amcl Couldn't determine robot's pose associated with laser scan";
//          }
          return -3;
        }
    }

//    clock_gettime(CLOCK_MONOTONIC, &endTime2);
//    currentConsumed=1000000000*(endTime2.tv_sec-startTime.tv_sec)+endTime2.tv_nsec-startTime.tv_nsec;
//    currentConsumed=currentConsumed/1000.0;
//    if(debugFlag)
//    {
//        qDebug()<<"amcl endTime2:"<<currentConsumed;
//    }

    //初始化ａgv位置时，需要多次收敛。
    bool tmpIsInitialAgvPosition=false;
    if(initialAgvPositionCount>0)
    {
        initialAgvPositionCount--;
        tmpIsInitialAgvPosition=true;
        printf("initial agv pose %d",initialAgvPositionCount);//no print
        qDebug()<<"force initial agv pose"<<initialAgvPositionCount;
    }

    pf_vector_t delta = pf_vector_zero();

     qDebug()<<"2begin: pf_init_"<<pf_init_;
    if(pf_init_)
    {
      // Compute change in pose
      //delta = pf_vector_coord_sub(pose, pf_odom_pose_);
      delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
      delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
      delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);

      // See if we should update the filter
      static bool update;
      if(0==calculateCount)
      {
          update= fabs(delta.v[0]) > d_thresh_ ||
                        fabs(delta.v[1]) > d_thresh_ ||
                        fabs(delta.v[2]) > a_thresh_;
          qDebug()<<"delta.v[0]"<<delta.v[0]<<"delta.v[1]"<<delta.v[1]<<"delta.v[2]"<<delta.v[2];
      }

//      //hualei 每多少次更新一次
//      static int tmp_count=0;
//      bool tmp_isUpdate=false;
//      if(isUpdatePartialEveryTime>0)
//      {
//          tmp_count++;
//          if(tmp_count==isUpdatePartialEveryTime)
//          {
//              tmp_isUpdate=true;
//              tmp_count=0;
//          }
//      }

     qDebug()<<"1begin: update"<<update<<"m_force_update"<<m_force_update
            <<"tmpIsInitialAgvPosition"<<tmpIsInitialAgvPosition;
      update = update || m_force_update  || tmpIsInitialAgvPosition;//hualei
      m_force_update=false;


      // Set the laser update flags
      if(update)
      {
          for(unsigned int i=0; i < lasers_update_.size(); i++)
            lasers_update_[i] = true;
      }

    }



//    clock_gettime(CLOCK_MONOTONIC, &endTime3);
//    currentConsumed=1000000000*(endTime3.tv_sec-startTime.tv_sec)+endTime3.tv_nsec-startTime.tv_nsec;
//    currentConsumed=currentConsumed/1000.0;
//    if(debugFlag)
//    {
//        qDebug()<<"amcl endTime3:"<<currentConsumed;
//    }

    force_publication = false;
    if(!pf_init_)
    {
      // Pose at last filter update
      pf_odom_pose_ = pose;

      // Filter is now initialized
      pf_init_ = true;

      // Should update sensor data
      for(unsigned int i=0; i < lasers_update_.size(); i++)
        lasers_update_[i] = true;

      force_publication = true;

      resample_count_ = 0;

    }
    // If the robot has moved, update the filter
    else if(pf_init_ && lasers_update_[laser_index])
    {
        qDebug()<<"3begin: result lasers_update_"<<true;
      //printf("pose\n");
      //pf_vector_fprintf(pose, stdout, "%.3f");

      AMCLOdomData odata;
      odata.pose = pose;
      // HACK
      // Modify the delta in the action data so the filter gets
      // updated correctly
      odata.delta = delta;
      //华磊，除了第一次计算里程计有变化，其它次计算里程计没有变化。
      if(0!=calculateCount)
      {
          odata.delta = pf_vector_zero();
      }

      // Use the action data to update the filter.根据里程计变化更新粒子分布。
      odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);

      // Pose at last filter update
      //this->pf_odom_pose = pose;
    }

//    clock_gettime(CLOCK_MONOTONIC, &endTime4);
//    currentConsumed=1000000000*(endTime4.tv_sec-startTime.tv_sec)+endTime4.tv_nsec-startTime.tv_nsec;
//    currentConsumed=currentConsumed/1000.0;
//    if(debugFlag)
//    {
//        qDebug()<<"amcl endTime4:"<<currentConsumed;
//    }

    bool resampled = false;
    // If the robot has moved, update the filter
    if(lasers_update_[laser_index])
    {
      AMCLLaserData ldata;
      ldata.sensor = lasers_[laser_index];
      ldata.range_count = laser_scan->ranges.size();

      // To account for lasers that are mounted upside-down, we determine the
      // min, max, and increment angles of the laser in the base frame.
      //
      // Construct min and max angles of laser, in the base_link frame.
      tf::Quaternion q;
      q.setRPY(0.0, 0.0, laser_scan->angle_min);
      tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
                                        laser_scan->header.frame_id);
      q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
      tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
                                        laser_scan->header.frame_id);
      try
      {
        tf_->transformQuaternion(base_frame_id_, min_q, min_q);
        tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
      }
      catch(tf::TransformException& e)
      {
        ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
                 e.what());
        if(1)
        {
            qDebug()<<"amcl failed4,Unable to transform min/max laser angles into base frame";
        }
        return -4;
      }


      double angle_min = tf::getYaw(min_q);
      double angle_increment = tf::getYaw(inc_q) - angle_min;

      // wrapping angle to [-pi .. pi]
      angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;

      ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);

      // Apply range min/max thresholds, if the user supplied them
      if(laser_max_range_ > 0.0)
        ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
      else
        ldata.range_max = laser_scan->range_max;
      double range_min;
      if(laser_min_range_ > 0.0)
        range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
      else
        range_min = laser_scan->range_min;
      // The AMCLLaserData destructor will free this memory
      ldata.ranges = new double[ldata.range_count][2];
      ROS_ASSERT(ldata.ranges);
      for(int i=0;i<ldata.range_count;i++)
      {
        // amcl doesn't (yet) have a concept of min range.  So we'll map short
        // readings to max range.
        if(laser_scan->ranges[i] <= range_min)
          ldata.ranges[i][0] = ldata.range_max;
        else
          ldata.ranges[i][0] = laser_scan->ranges[i];
        // Compute bearing
        ldata.ranges[i][1] = angle_min +
                (i * angle_increment);
      }

      clock_gettime(CLOCK_MONOTONIC, &endTime5a);
      currentConsumed=1000000000*(endTime5a.tv_sec-startTime.tv_sec)+endTime5a.tv_nsec-startTime.tv_nsec;
      currentConsumed=currentConsumed/1000.0;
      if(debugFlag)
      {
          qDebug()<<"amcl endTime5a:"<<currentConsumed;
      }

      lasers_[laser_index]->UpdateSensor(&positionCertaintyFactor,pf_, (AMCLSensorData*)&ldata);
  //    positionCertaintyFactor=lasers_[laser_index]->calculateDistanceBetweenLaserAndMap((AMCLLaserData*)&ldata,current_pose_mean);//hualei 计算地图偏差

      lasers_update_[laser_index] = false;

      pf_odom_pose_ = pose;

      clock_gettime(CLOCK_MONOTONIC, &endTime5b);
      currentConsumed=1000000000*(endTime5b.tv_sec-startTime.tv_sec)+endTime5b.tv_nsec-startTime.tv_nsec;
      currentConsumed=currentConsumed/1000.0;
      if(debugFlag)
      {
          //40ms,与里程计误差参数无关．粒子数降低到１０，cpu达到１４０％，最长时间变短了．
          qDebug()<<"amcl UpdateSensor endTime5b:"<<currentConsumed;
      }


      // Resample the particles
      if(!(++resample_count_ % resample_interval_))
      {
        pf_update_resample(pf_);
        resampled = true;
      }
//        if(calculateCount==calculateTimesForLaser-1)
//        {
//          pf_update_resample(pf_);
//          resampled = true;
//        }

      pf_sample_set_t* set = pf_->sets + pf_->current_set;
      ROS_DEBUG("Num samples: %d\n", set->sample_count);

      clock_gettime(CLOCK_MONOTONIC, &endTime5c);
      currentConsumed=1000000000*(endTime5c.tv_sec-startTime.tv_sec)+endTime5c.tv_nsec-startTime.tv_nsec;
      currentConsumed=currentConsumed/1000.0;
      if(debugFlag)
      {
          //20ms
          qDebug()<<"amcl pf_update_resample endTime5c:"<<currentConsumed<<"sample_count"<<set->sample_count;
      }

      // Publish the resulting cloud
      // TODO: set maximum rate for publishing
      if (!m_force_update) {
        geometry_msgs::PoseArray cloud_msg;
        cloud_msg.header.stamp = ros::Time::now();
        cloud_msg.header.frame_id = global_frame_id_;
        cloud_msg.poses.resize(set->sample_count);
        for(int i=0;i<set->sample_count;i++)
        {
          tf::poseTFToMsg(tf::Pose(tf::createQuaternionFromYaw(set->samples[i].pose.v[2]),
                                   tf::Vector3(set->samples[i].pose.v[0],
                                             set->samples[i].pose.v[1], 0)),
                          cloud_msg.poses[i]);
        }
        particlecloud_pub_.publish(cloud_msg);
      }
    }

    clock_gettime(CLOCK_MONOTONIC, &endTime5);
    currentConsumed=1000000000*(endTime5.tv_sec-startTime.tv_sec)+endTime5.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(debugFlag)
    {
        qDebug()<<"amcl endTime5:"<<currentConsumed;
    }

    if(resampled || force_publication)
    {
      if (!resampled)
      {
          // re-compute the cluster statistics
          int tmpReturn = pf_cluster_stats(pf_, pf_->sets);
          if(1!=tmpReturn)//hualei
          {
              isPfOk=false;
//              if(debugFlag)
//              {
                  qDebug()<<"amcl failed5,pf_cluster_stats";
//              }
              return -5;
          }
          else
          {
              isPfOk=true;
          }
      }
      // Read out the current hypotheses
      double max_weight = 0.0;
      int max_weight_hyp = -1;
      std::vector<amcl_hyp_t> hyps;
      hyps.resize(pf_->sets[pf_->current_set].cluster_count);
      for(int hyp_count = 0;
          hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
      {
        double weight;
        pf_vector_t pose_mean;
        pf_matrix_t pose_cov;
        if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
        {
          ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
//          if(debugFlag)
//          {
              qDebug()<<"amcl failed?,Couldn't get stats on cluster";
//          }
          break;
        }

        hyps[hyp_count].weight = weight;
        hyps[hyp_count].pf_pose_mean = pose_mean;
        hyps[hyp_count].pf_pose_cov = pose_cov;
        current_pose_mean= pose_mean;//hualei

        if(hyps[hyp_count].weight > max_weight)
        {
          max_weight = hyps[hyp_count].weight;
          max_weight_hyp = hyp_count;
        }
      }

      if(max_weight > 0.0)
      {
        socre_weight=max_weight;//hualei
        socre_pos_cov[0]=hyps[max_weight_hyp].pf_pose_cov.m[0][0] ;//hualei
        socre_pos_cov[1]=hyps[max_weight_hyp].pf_pose_cov.m[0][1] ;//hualei
        socre_pos_cov[2]=hyps[max_weight_hyp].pf_pose_cov.m[0][2] ;//hualei
        socre_pos_cov[3]=hyps[max_weight_hyp].pf_pose_cov.m[1][0] ;//hualei
        socre_pos_cov[4]=hyps[max_weight_hyp].pf_pose_cov.m[1][1] ;//hualei
        socre_pos_cov[5]=hyps[max_weight_hyp].pf_pose_cov.m[1][2] ;//hualei
        socre_pos_cov[6]=hyps[max_weight_hyp].pf_pose_cov.m[2][0] ;//hualei
        socre_pos_cov[7]=hyps[max_weight_hyp].pf_pose_cov.m[2][1] ;//hualei
        socre_pos_cov[8]=hyps[max_weight_hyp].pf_pose_cov.m[2][3] ;//hualei
        ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
                  hyps[max_weight_hyp].pf_pose_mean.v[0],
                  hyps[max_weight_hyp].pf_pose_mean.v[1],
                  hyps[max_weight_hyp].pf_pose_mean.v[2]);

        /*
           puts("");
           pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
           puts("");
         */

        geometry_msgs::PoseWithCovarianceStamped p;
        // Fill in the header
        p.header.frame_id = global_frame_id_;
        p.header.stamp = laser_scan->header.stamp;

        if(isCorrectByReflect)
        {
            calculateCorrectPosition(reflectPosition,hyps[max_weight_hyp].pf_pose_mean.v[0]
                                        ,hyps[max_weight_hyp].pf_pose_mean.v[1],
                                        hyps[max_weight_hyp].pf_pose_mean.v[2]);
        }

        // Copy in the pose
        p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
        p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
        tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                              p.pose.pose.orientation);
        // Copy in the covariance, converting from 3-D to 6-D
        pf_sample_set_t* set = pf_->sets + pf_->current_set;
        for(int i=0; i<2; i++)
        {
          for(int j=0; j<2; j++)
          {
            // Report the overall filter covariance, rather than the
            // covariance for the highest-weight cluster
            //p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
            p.pose.covariance[6*i+j] = set->cov.m[i][j];
          }
        }
        // Report the overall filter covariance, rather than the
        // covariance for the highest-weight cluster
        //p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
        p.pose.covariance[6*5+5] = set->cov.m[2][2];

        /*
           printf("cov:\n");
           for(int i=0; i<6; i++)
           {
           for(int j=0; j<6; j++)
           printf("%6.3f ", p.covariance[6*i+j]);
           puts("");
           }
         */



        pose_pub_.publish(p);
        last_published_pose = p;

        isCalculateSucess=true;

//        qDebug()<<"isCalculateSucess";

        ROS_DEBUG("New amcl origin pose: %6.3f %6.3f %6.3f",
                 hyps[max_weight_hyp].pf_pose_mean.v[0],
                 hyps[max_weight_hyp].pf_pose_mean.v[1],
                 hyps[max_weight_hyp].pf_pose_mean.v[2]);

        // subtracting base to odom from map to base and send map to odom instead
        tf::Stamped<tf::Pose> odom_to_map;
        try
        {
          tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                               tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
                                           hyps[max_weight_hyp].pf_pose_mean.v[1],
                                           0.0));
          tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
                                                laser_scan->header.stamp,
                                                base_frame_id_);
          this->tf_->transformPose(odom_frame_id_,
                                   tmp_tf_stamped,
                                   odom_to_map);
        }
        catch(tf::TransformException)
        {
          ROS_DEBUG("Failed to subtract base to odom transform");
//          if(debugFlag)
//          {
              qDebug()<<"amcl failed6,Failed to subtract base to odom transform";
//          }
          return -6;
        }

        // odom_to_map
        usingMutex.lock();//
        calculateOffsetMatrix_tf(odom_to_map,frame_odom_in_map);
        usingMutex.unlock();//
        latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
                                   tf::Point(odom_to_map.getOrigin()));
        latest_tf_valid_ = true;

        if (tf_broadcast_ == true)
        {
          // We want to send a transform that is good up until a
          // tolerance time so that odom can be used
          ros::Time transform_expiration = (laser_scan->header.stamp +
                                            transform_tolerance_);
          tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
                                              transform_expiration,
                                              global_frame_id_, odom_frame_id_);
          this->tfb_->sendTransform(tmp_tf_stamped);
          sent_first_transform_ = true;
  //        mapMatchScore_pub_.publish(mathWightToPub);//modify by hualei
        }
      }
      else
      {
        ROS_ERROR("No pose!");
//        if(debugFlag)
//        {
            qDebug()<<"amcl failed?,No pose";
//        }
      }

      clock_gettime(CLOCK_MONOTONIC, &endTime6);
      currentConsumed=1000000000*(endTime6.tv_sec-startTime.tv_sec)+endTime6.tv_nsec-startTime.tv_nsec;
      currentConsumed=currentConsumed/1000.0;
      if(debugFlag)
      {
          qDebug()<<"amcl endTime6:"<<currentConsumed;
      }

    }
    else if(latest_tf_valid_)
    {
      //广播tf导致cpu高.和位置不对没有关系。
//      if (tf_broadcast_ == true)
//      {
        // Nothing changed, so we'll just republish the last transform, to keep
        // everybody happy.
//        ros::Time transform_expiration = (laser_scan->header.stamp +
//                                          transform_tolerance_);
//        tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
//                                            transform_expiration,
//                                            global_frame_id_, odom_frame_id_);
//        this->tfb_->sendTransform(tmp_tf_stamped);
//  //      mapMatchScore_pub_.publish(mathWightToPub);//modify by hualei
//        clock_gettime(CLOCK_MONOTONIC, &endTime6);
//        currentConsumed=1000000000*(endTime6.tv_sec-startTime.tv_sec)+endTime6.tv_nsec-startTime.tv_nsec;
//        currentConsumed=currentConsumed/1000.0;
//        if(debugFlag)
//        {
//            qDebug()<<"amcl endTime6:"<<currentConsumed;
//        }
//      }

      // Is it time to save our last pose to the param server
      ros::Time now = ros::Time::now();
      if((save_pose_period.toSec() > 0.0) &&
         (now - save_pose_last_time) >= save_pose_period)
      {
//        this->savePoseToServer();
//        save_pose_last_time = now;
//        clock_gettime(CLOCK_MONOTONIC, &endTime7);
//        currentConsumed=1000000000*(endTime7.tv_sec-startTime.tv_sec)+endTime7.tv_nsec-startTime.tv_nsec;
//        currentConsumed=currentConsumed/1000.0;
//        if(debugFlag)
//        {
//            qDebug()<<"amcl endTime7:"<<currentConsumed;
//        }
      }
    }

    qDebug()<<"end: resampled"<<resampled<<"force_publication"<<force_publication
                <<"force_publication"<<force_publication<<"pf_init_"<<pf_init_;

    return isCalculateSucess;
}

int AmclNode::laserDataCalculate_internalICP(const sensor_msgs::LaserScanConstPtr &laser_scan,int calculateTimesIn
                                             ,bool& isReSetCloudOut)
{
//    qDebug()<<"AmclNode::laserDataCalculate_internalICP";
    if(debugFlag)
    {
        qDebug()<<"AmclNode::laserDataCalculate_internalICP";
    }
    //时间分析
    struct timespec startTime,endTime7,endTime2,endTime3,endTime4,endTime5,endTime6
            ,endTime5a,endTime5b,endTime5c;
    clock_gettime(CLOCK_MONOTONIC, &startTime);
    double currentConsumed;

    isReSetCloudOut=true;
    bool isCalculateSucess=false;

    if( map_ == NULL )
    {
        if(1)
        {
            qDebug()<<"amcl failed1,map_ == NULL";
        }
      return -1;
    }
    boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
    int laser_index = -1;

    if(!pf_init_)
    {
        isReSetCloudOut=false;
    }
    //0此处开始循环时，粒子收缩。
//    bool resampled = false;
//    for(int i=0;i<calculateTimesIn;i++)
//    {

    // Do we have the base->base_laser Tx yet?
    if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end())
    {
      ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan->header.frame_id.c_str());
      lasers_.push_back(new AMCLLaser(*laser_));
      lasers_update_.push_back(true);
      laser_index = frame_to_laser_.size();

      tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
                                               tf::Vector3(0,0,0)),
                                   ros::Time(), laser_scan->header.frame_id);
      tf::Stamped<tf::Pose> laser_pose;
      try
      {
        this->tf_->transformPose(base_frame_id_, ident, laser_pose);
      }
      catch(tf::TransformException& e)
      {
        ROS_ERROR("Couldn't transform from %s to %s, "
                  "even though the message notifier is in use",
                  laser_scan->header.frame_id.c_str(),
                  base_frame_id_.c_str());
        if(1)
        {
            qDebug()<<"amcl failed2,TransformException";
        }
        return -2;
      }

      static pf_vector_t laser_pose_v;

      laser_pose_v.v[0] = laser_pose.getOrigin().x();
      laser_pose_v.v[1] = laser_pose.getOrigin().y();
      // laser mounting angle gets computed later -> set to 0 here!
      laser_pose_v.v[2] = 0;


      lasers_[laser_index]->SetLaserPose(laser_pose_v);
      ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
                laser_pose_v.v[0],
                laser_pose_v.v[1],
                laser_pose_v.v[2]);

      frame_to_laser_[laser_scan->header.frame_id] = laser_index;
    }
    else
    {
      // we have the laser pose, retrieve laser index
      laser_index = frame_to_laser_[laser_scan->header.frame_id];
    }

//    clock_gettime(CLOCK_MONOTONIC, &endTime1);
//    currentConsumed=1000000000*(endTime1.tv_sec-startTime.tv_sec)+endTime1.tv_nsec-startTime.tv_nsec;
//    currentConsumed=currentConsumed/1000.0;
//    if(debugFlag)
//    {
//        qDebug()<<"amcl endTime1:"<<currentConsumed;
//    }

    // Where was the robot when this scan was taken?
    static pf_vector_t pose;//需要保证同一帧激光数据的多次计算里程计一直不变。

    //可能导致ros位置失败．
    if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
                    laser_scan->header.stamp, base_frame_id_))
    {
//          ROS_ERROR("amcl Couldn't determine robot's pose associated with laser scan");
//          if(debugFlag)
//          {
        if(initialAgvPositionCount>0)
        {
            initialAgvPositionCount--;
        }
        qDebug()<<"amcl failed3,amcl Couldn't determine robot's pose associated with laser scan";
//          }
      return -3;
    }


//    clock_gettime(CLOCK_MONOTONIC, &endTime2);
//    currentConsumed=1000000000*(endTime2.tv_sec-startTime.tv_sec)+endTime2.tv_nsec-startTime.tv_nsec;
//    currentConsumed=currentConsumed/1000.0;
//    if(debugFlag)
//    {
//        qDebug()<<"amcl endTime2:"<<currentConsumed;
//    }

    //初始化ａgv位置时，需要多次收敛。
    bool tmpIsInitialAgvPosition=false;
    if(initialAgvPositionCount>0)
    {
        initialAgvPositionCount--;
        tmpIsInitialAgvPosition=true;
        printf("initial agv pose %d",initialAgvPositionCount);//no print
        qDebug()<<"force initial agv pose"<<initialAgvPositionCount;
    }

    pf_vector_t delta = pf_vector_zero();

//     qDebug()<<"2begin: pf_init_"<<pf_init_;
    static bool update;
    bool resampled = false;
//    for(int i=0;i<calculateTimesIn;i++)
//    {

    if(pf_init_)
    {
      // Compute change in pose
      //delta = pf_vector_coord_sub(pose, pf_odom_pose_);
      delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
      delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
      delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);

      // See if we should update the filter

//      if(0==i)
//      {
          update= fabs(delta.v[0]) > d_thresh_ ||
                        fabs(delta.v[1]) > d_thresh_ ||
                        fabs(delta.v[2]) > a_thresh_;
//      }


//      qDebug()<<"delta.v[0]"<<delta.v[0]<<"delta.v[1]"<<delta.v[1]<<"delta.v[2]"<<delta.v[2];


//      //hualei 每多少次更新一次
//      static int tmp_count=0;
//      bool tmp_isUpdate=false;
//      if(isUpdatePartialEveryTime>0)
//      {
//          tmp_count++;
//          if(tmp_count==isUpdatePartialEveryTime)
//          {
//              tmp_isUpdate=true;
//              tmp_count=0;
//          }
//      }

//     qDebug()<<"1begin: update"<<update<<"m_force_update"<<m_force_update
//            <<"tmpIsInitialAgvPosition"<<tmpIsInitialAgvPosition;
      update = update || m_force_update  || tmpIsInitialAgvPosition;//hualei
      m_force_update=false;


      // Set the laser update flags
      if(update)
      {
          for(unsigned int i=0; i < lasers_update_.size(); i++)
            lasers_update_[i] = true;
      }
      else
      {
          for(unsigned int i=0; i < lasers_update_.size(); i++)
            lasers_update_[i] = false;
      }

    }

//    clock_gettime(CLOCK_MONOTONIC, &endTime3);
//    currentConsumed=1000000000*(endTime3.tv_sec-startTime.tv_sec)+endTime3.tv_nsec-startTime.tv_nsec;
//    currentConsumed=currentConsumed/1000.0;
//    if(debugFlag)
//    {
//        qDebug()<<"amcl endTime3:"<<currentConsumed;
//    }

    //1此处开始循环时，粒子不收缩。
    force_publication = false;

        if(!pf_init_)
        {
              // Pose at last filter update
              pf_odom_pose_ = pose;

              // Filter is now initialized
              pf_init_ = true;

              // Should update sensor data
              for(unsigned int i=0; i < lasers_update_.size(); i++)
                lasers_update_[i] = true;

              force_publication = true;

              resample_count_ = 0;
              qDebug()<<"!pf_init_,lasers_update_[i] = true";

        }
        // If the robot has moved, update the filter
        else if(pf_init_ && lasers_update_[laser_index])
        {
//                qDebug()<<"pf_init_ && lasers_update_[laser_index]"<<i;
              //printf("pose\n");
              //pf_vector_fprintf(pose, stdout, "%.3f");

              AMCLOdomData odata;
              odata.pose = pose;
              // HACK
              // Modify the delta in the action data so the filter gets
              // updated correctly
              odata.delta = delta;
              //华磊，除了第一次计算里程计有变化，其它次计算里程计没有变化。
//              if(0!=i)
//              {
//                  odata.delta = pf_vector_zero();
//              }
//              else
//              {
                  // Use the action data to update the filter.根据里程计变化移动粒子。
                  odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
//              }



              // Pose at last filter update
              //this->pf_odom_pose = pose;
        }

    //    clock_gettime(CLOCK_MONOTONIC, &endTime4);
    //    currentConsumed=1000000000*(endTime4.tv_sec-startTime.tv_sec)+endTime4.tv_nsec-startTime.tv_nsec;
    //    currentConsumed=currentConsumed/1000.0;
    //    if(debugFlag)
    //    {
    //        qDebug()<<"amcl endTime4:"<<currentConsumed;
    //    }


        // If the robot has moved, update the filter

        if(lasers_update_[laser_index])
        {
//              qDebug()<<"lasers_update_[laser_index] moved icp"<<i;
              AMCLLaserData ldata;
              ldata.sensor = lasers_[laser_index];
              ldata.range_count = laser_scan->ranges.size();

              // To account for lasers that are mounted upside-down, we determine the
              // min, max, and increment angles of the laser in the base frame.
              //
              // Construct min and max angles of laser, in the base_link frame.
              tf::Quaternion q;
              q.setRPY(0.0, 0.0, laser_scan->angle_min);
              tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
                                                laser_scan->header.frame_id);
              q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
              tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
                                                laser_scan->header.frame_id);
              try
              {
                tf_->transformQuaternion(base_frame_id_, min_q, min_q);
                tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
              }
              catch(tf::TransformException& e)
              {
                ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
                         e.what());
                if(1)
                {
                    qDebug()<<"amcl failed4,Unable to transform min/max laser angles into base frame";
                }
                return -4;
              }


              double angle_min = tf::getYaw(min_q);
              double angle_increment = tf::getYaw(inc_q) - angle_min;

              // wrapping angle to [-pi .. pi]
              angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;

              ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);

              // Apply range min/max thresholds, if the user supplied them
              if(laser_max_range_ > 0.0)
                ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
              else
                ldata.range_max = laser_scan->range_max;
              double range_min;
              if(laser_min_range_ > 0.0)
                range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
              else
                range_min = laser_scan->range_min;
              // The AMCLLaserData destructor will free this memory
              ldata.ranges = new double[ldata.range_count][2];
              ROS_ASSERT(ldata.ranges);
              for(int i=0;i<ldata.range_count;i++)
              {
                // amcl doesn't (yet) have a concept of min range.  So we'll map short
                // readings to max range.
                if(laser_scan->ranges[i] <= range_min)
                  ldata.ranges[i][0] = ldata.range_max;
                else
                  ldata.ranges[i][0] = laser_scan->ranges[i];
                // Compute bearing
                ldata.ranges[i][1] = angle_min +
                        (i * angle_increment);
              }

              clock_gettime(CLOCK_MONOTONIC, &endTime5a);
              currentConsumed=1000000000*(endTime5a.tv_sec-startTime.tv_sec)+endTime5a.tv_nsec-startTime.tv_nsec;
              currentConsumed=currentConsumed/1000.0;
              if(debugFlag)
              {
                  qDebug()<<"amcl endTime5a:"<<currentConsumed;
              }
              //复制一个临时pf，仅仅用于收敛，计算精确位置和发布粒子。
//              pf_t *tmp_pf;
//              pf_clone(pf_,tmp_pf);
              for(int i=0;i<calculateTimesIn;i++)
              {
                  //计算粒子权重
                  lasers_[laser_index]->UpdateSensor(&positionCertaintyFactor,pf_, (AMCLSensorData*)&ldata);
                  lasers_update_[laser_index] = false;
                  pf_odom_pose_ = pose;
                  // Resample the particles
                  pf_update_resample(pf_);
                  resampled = true;
              }

//              pf_free( tmp_pf_ );

                clock_gettime(CLOCK_MONOTONIC, &endTime5b);
                currentConsumed=1000000000*(endTime5b.tv_sec-startTime.tv_sec)+endTime5b.tv_nsec-startTime.tv_nsec;
                currentConsumed=currentConsumed/1000.0;
                if(debugFlag)
                {
                    //40ms,与里程计误差参数无关．粒子数降低到１０，cpu达到１４０％，最长时间变短了．
                    qDebug()<<"amcl UpdateSensor endTime5b:"<<currentConsumed;
                }

              pf_sample_set_t* set = pf_->sets + pf_->current_set;
              ROS_DEBUG("Num samples: %d\n", set->sample_count);

              clock_gettime(CLOCK_MONOTONIC, &endTime5c);
              currentConsumed=1000000000*(endTime5c.tv_sec-startTime.tv_sec)+endTime5c.tv_nsec-startTime.tv_nsec;
              currentConsumed=currentConsumed/1000.0;
              if(debugFlag)
              {
                  //20ms
                  qDebug()<<"amcl pf_update_resample endTime5c:"<<currentConsumed<<"sample_count"<<set->sample_count;
              }

              // Publish the resulting cloud
              // TODO: set maximum rate for publishing
              if (!m_force_update)
              {
                geometry_msgs::PoseArray cloud_msg;
                cloud_msg.header.stamp = ros::Time::now();
                cloud_msg.header.frame_id = global_frame_id_;
                cloud_msg.poses.resize(set->sample_count);
                for(int i=0;i<set->sample_count;i++)
                {
                  tf::poseTFToMsg(tf::Pose(tf::createQuaternionFromYaw(set->samples[i].pose.v[2]),
                                           tf::Vector3(set->samples[i].pose.v[0],
                                                     set->samples[i].pose.v[1], 0)),
                                  cloud_msg.poses[i]);
                }
                particlecloud_pub_.publish(cloud_msg);
              }
        }

        clock_gettime(CLOCK_MONOTONIC, &endTime5);
        currentConsumed=1000000000*(endTime5.tv_sec-startTime.tv_sec)+endTime5.tv_nsec-startTime.tv_nsec;
        currentConsumed=currentConsumed/1000.0;
        if(debugFlag)
        {
            qDebug()<<"amcl endTime5:"<<currentConsumed;
        }

        //根据粒子求位置
        if(resampled || force_publication)
        {
              if (!resampled)
              {
                  // re-compute the cluster statistics
                  int tmpReturn = pf_cluster_stats(pf_, pf_->sets);
                  if(1!=tmpReturn)//hualei
                  {
                      isPfOk=false;
        //              if(debugFlag)
        //              {
                          qDebug()<<"amcl failed5,pf_cluster_stats";
        //              }
                      return -5;
                  }
                  else
                  {
                      isPfOk=true;
                  }
              }
              // Read out the current hypotheses
              double max_weight = 0.0;
              int max_weight_hyp = -1;
              std::vector<amcl_hyp_t> hyps;
              hyps.resize(pf_->sets[pf_->current_set].cluster_count);
              for(int hyp_count = 0;
                  hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
              {
                double weight;
                pf_vector_t pose_mean;
                pf_matrix_t pose_cov;
                if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
                {
                  ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
        //          if(debugFlag)
        //          {
                      qDebug()<<"amcl failed?,Couldn't get stats on cluster";
        //          }
                  break;
                }

                hyps[hyp_count].weight = weight;
                hyps[hyp_count].pf_pose_mean = pose_mean;
                hyps[hyp_count].pf_pose_cov = pose_cov;
                current_pose_mean= pose_mean;//hualei

                if(hyps[hyp_count].weight > max_weight)
                {
                  max_weight = hyps[hyp_count].weight;
                  max_weight_hyp = hyp_count;
                }
              }

              if(max_weight > 0.0)
              {
                socre_weight=max_weight;//hualei
                socre_pos_cov[0]=hyps[max_weight_hyp].pf_pose_cov.m[0][0] ;//hualei
                socre_pos_cov[1]=hyps[max_weight_hyp].pf_pose_cov.m[0][1] ;//hualei
                socre_pos_cov[2]=hyps[max_weight_hyp].pf_pose_cov.m[0][2] ;//hualei
                socre_pos_cov[3]=hyps[max_weight_hyp].pf_pose_cov.m[1][0] ;//hualei
                socre_pos_cov[4]=hyps[max_weight_hyp].pf_pose_cov.m[1][1] ;//hualei
                socre_pos_cov[5]=hyps[max_weight_hyp].pf_pose_cov.m[1][2] ;//hualei
                socre_pos_cov[6]=hyps[max_weight_hyp].pf_pose_cov.m[2][0] ;//hualei
                socre_pos_cov[7]=hyps[max_weight_hyp].pf_pose_cov.m[2][1] ;//hualei
                socre_pos_cov[8]=hyps[max_weight_hyp].pf_pose_cov.m[2][3] ;//hualei
                ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
                          hyps[max_weight_hyp].pf_pose_mean.v[0],
                          hyps[max_weight_hyp].pf_pose_mean.v[1],
                          hyps[max_weight_hyp].pf_pose_mean.v[2]);

                /*
                   puts("");
                   pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
                   puts("");
                 */

                geometry_msgs::PoseWithCovarianceStamped p;
                // Fill in the header
                p.header.frame_id = global_frame_id_;
                p.header.stamp = laser_scan->header.stamp;

                if(isCorrectByReflect)
                {
                    calculateCorrectPosition(reflectPosition,hyps[max_weight_hyp].pf_pose_mean.v[0]
                                                ,hyps[max_weight_hyp].pf_pose_mean.v[1],
                                                hyps[max_weight_hyp].pf_pose_mean.v[2]);
                }

                // Copy in the pose
                p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
                p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
                tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                                      p.pose.pose.orientation);
                // Copy in the covariance, converting from 3-D to 6-D
                pf_sample_set_t* set = pf_->sets + pf_->current_set;
                for(int i=0; i<2; i++)
                {
                  for(int j=0; j<2; j++)
                  {
                    // Report the overall filter covariance, rather than the
                    // covariance for the highest-weight cluster
                    //p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
                    p.pose.covariance[6*i+j] = set->cov.m[i][j];
                  }
                }
                // Report the overall filter covariance, rather than the
                // covariance for the highest-weight cluster
                //p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
                p.pose.covariance[6*5+5] = set->cov.m[2][2];

                /*
                   printf("cov:\n");
                   for(int i=0; i<6; i++)
                   {
                   for(int j=0; j<6; j++)
                   printf("%6.3f ", p.covariance[6*i+j]);
                   puts("");
                   }
                 */



                pose_pub_.publish(p);
                last_published_pose = p;

                isCalculateSucess=true;

        //        qDebug()<<"isCalculateSucess";

                ROS_DEBUG("New amcl origin pose: %6.3f %6.3f %6.3f",
                         hyps[max_weight_hyp].pf_pose_mean.v[0],
                         hyps[max_weight_hyp].pf_pose_mean.v[1],
                         hyps[max_weight_hyp].pf_pose_mean.v[2]);

                // subtracting base to odom from map to base and send map to odom instead
                tf::Stamped<tf::Pose> odom_to_map;
                try
                {
                  tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
                                       tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
                                                   hyps[max_weight_hyp].pf_pose_mean.v[1],
                                                   0.0));
                  tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
                                                        laser_scan->header.stamp,
                                                        base_frame_id_);
                  this->tf_->transformPose(odom_frame_id_,
                                           tmp_tf_stamped,
                                           odom_to_map);
                }
                catch(tf::TransformException)
                {
                  ROS_DEBUG("Failed to subtract base to odom transform");
        //          if(debugFlag)
        //          {
                      qDebug()<<"amcl failed6,Failed to subtract base to odom transform";
        //          }
                  return -6;
                }

                // odom_to_map
                usingMutex.lock();//
                calculateOffsetMatrix_tf(odom_to_map,frame_odom_in_map);
                usingMutex.unlock();//
                latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
                                           tf::Point(odom_to_map.getOrigin()));
                latest_tf_valid_ = true;

                if (tf_broadcast_ == true)
                {
                  // We want to send a transform that is good up until a
                  // tolerance time so that odom can be used
                  ros::Time transform_expiration = (laser_scan->header.stamp +
                                                    transform_tolerance_);
                  tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
                                                      transform_expiration,
                                                      global_frame_id_, odom_frame_id_);
                  this->tfb_->sendTransform(tmp_tf_stamped);
                  sent_first_transform_ = true;
          //        mapMatchScore_pub_.publish(mathWightToPub);//modify by hualei
                }
              }
              else
              {
                ROS_ERROR("No pose!");
        //        if(debugFlag)
        //        {
                    qDebug()<<"amcl failed?,No pose";
        //        }
              }

              clock_gettime(CLOCK_MONOTONIC, &endTime6);
              currentConsumed=1000000000*(endTime6.tv_sec-startTime.tv_sec)+endTime6.tv_nsec-startTime.tv_nsec;
              currentConsumed=currentConsumed/1000.0;
              if(debugFlag)
              {
                  qDebug()<<"amcl endTime6:"<<currentConsumed;
              }

        }
        else if(latest_tf_valid_)
        {
          //广播tf导致cpu高.和位置不对没有关系。
    //      if (tf_broadcast_ == true)
    //      {
            // Nothing changed, so we'll just republish the last transform, to keep
            // everybody happy.
    //        ros::Time transform_expiration = (laser_scan->header.stamp +
    //                                          transform_tolerance_);
    //        tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
    //                                            transform_expiration,
    //                                            global_frame_id_, odom_frame_id_);
    //        this->tfb_->sendTransform(tmp_tf_stamped);
    //  //      mapMatchScore_pub_.publish(mathWightToPub);//modify by hualei
    //        clock_gettime(CLOCK_MONOTONIC, &endTime6);
    //        currentConsumed=1000000000*(endTime6.tv_sec-startTime.tv_sec)+endTime6.tv_nsec-startTime.tv_nsec;
    //        currentConsumed=currentConsumed/1000.0;
    //        if(debugFlag)
    //        {
    //            qDebug()<<"amcl endTime6:"<<currentConsumed;
    //        }
    //      }

          // Is it time to save our last pose to the param server
          ros::Time now = ros::Time::now();
          if((save_pose_period.toSec() > 0.0) &&
             (now - save_pose_last_time) >= save_pose_period)
          {
    //        this->savePoseToServer();
    //        save_pose_last_time = now;
    //        clock_gettime(CLOCK_MONOTONIC, &endTime7);
    //        currentConsumed=1000000000*(endTime7.tv_sec-startTime.tv_sec)+endTime7.tv_nsec-startTime.tv_nsec;
    //        currentConsumed=currentConsumed/1000.0;
    //        if(debugFlag)
    //        {
    //            qDebug()<<"amcl endTime7:"<<currentConsumed;
    //        }
          }
        }

//    }


//    qDebug()<<"end: isCalculateSucess"<<isCalculateSucess<<"resampled"<<resampled
//           <<"force_publication"<<force_publication
//                <<"force_publication"<<force_publication<<"pf_init_"<<pf_init_;

    return isCalculateSucess;
}

void AmclNode::updatePoseFromServer()
{
  return ;
  init_pose_[0] = 0.0;
  init_pose_[1] = 0.0;
  init_pose_[2] = 0.0;
  init_cov_[0] = 0.5 * 0.5;
  init_cov_[1] = 0.5 * 0.5;
  init_cov_[2] = (M_PI/12.0) * (M_PI/12.0);
  // Check for NAN on input from param server, #5239
  double tmp_pos;
  private_nh_.param("initial_pose_x", tmp_pos, init_pose_[0]);
  if(!std::isnan(tmp_pos))
    init_pose_[0] = tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial pose X position");
  private_nh_.param("initial_pose_y", tmp_pos, init_pose_[1]);
  if(!std::isnan(tmp_pos))
    init_pose_[1] = tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial pose Y position");
  private_nh_.param("initial_pose_a", tmp_pos, init_pose_[2]);
  if(!std::isnan(tmp_pos))
    init_pose_[2] = tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial pose Yaw");
  private_nh_.param("initial_cov_xx", tmp_pos, init_cov_[0]);
  if(!std::isnan(tmp_pos))
    init_cov_[0] =tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial covariance XX");
  private_nh_.param("initial_cov_yy", tmp_pos, init_cov_[1]);
  if(!std::isnan(tmp_pos))
    init_cov_[1] = tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial covariance YY");
  private_nh_.param("initial_cov_aa", tmp_pos, init_cov_[2]);
  if(!std::isnan(tmp_pos))
    init_cov_[2] = tmp_pos;
  else
    ROS_WARN("ignoring NAN in initial covariance AA");	
}

void 
AmclNode::checkLaserReceived(const ros::TimerEvent& event)
{
    //加载大地图时会错误触发．
  if(true==isEnableToWork)
  {
      ros::Duration d = ros::Time::now() - last_laser_received_ts_;
      if(d > laser_check_interval_ && true == isEnableToWork)
      {
        ROS_WARN("No laser scan received (and thus no pose updates have been published) for %f seconds.  "
                 "Verify that data is being published on the %s topic.",
                 d.toSec(),
                 ros::names::resolve(scan_topic_).c_str());
        isLaserReceiveOk=false;
      }
      else
      {
          isLaserReceiveOk=true;
      }
  }

}

void
AmclNode::requestMap()
{
  boost::recursive_mutex::scoped_lock ml(configuration_mutex_);

  // get map via RPC
  nav_msgs::GetMap::Request  req;
  nav_msgs::GetMap::Response resp;
  ROS_INFO("Requesting the map...");
  while(!ros::service::call("static_map", req, resp))
  {
    ROS_WARN("Request for map failed; trying again...");
    ros::Duration d(0.5);
    d.sleep();
  }
  handleMapMessage( resp.map );
}

void
AmclNode::mapReceived(const nav_msgs::OccupancyGridConstPtr& msg)
{
  if( first_map_only_ && first_map_received_ ) {
    return;
  }

  handleMapMessage( *msg );

  first_map_received_ = true;
}

void
AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg)
{
  if(false==isEnableToWork)
  {
      return ;
  }
  boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);

  ROS_INFO("Received a %d X %d map @ %.3f m/pix\n",
           msg.info.width,
           msg.info.height,
           msg.info.resolution);
  
  if(msg.header.frame_id != global_frame_id_)
    ROS_WARN("Frame_id of map received:'%s' doesn't match global_frame_id:'%s;'. This could cause issues with reading published topics",
             msg.header.frame_id.c_str(),
             global_frame_id_.c_str());

  freeMapDependentMemory();
  // Clear queued laser objects because they hold pointers to the existing
  // map, #5202.
  lasers_.clear();
  lasers_update_.clear();
  frame_to_laser_.clear();

  map_ = convertMap(msg);

#if NEW_UNIFORM_SAMPLING
  // Index of free space
  free_space_indices.resize(0);
  for(int i = 0; i < map_->size_x; i++)
    for(int j = 0; j < map_->size_y; j++)
      if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)
        free_space_indices.push_back(std::make_pair(i,j));
#endif
  // Create the particle filter
  pf_ = pf_alloc(min_particles_, max_particles_,
                 alpha_slow_, alpha_fast_,
                 (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
                 (void *)map_);
  pf_->pop_err = pf_err_;
  pf_->pop_z = pf_z_;

  // Initialize the filter
  updatePoseFromServer();
  pf_vector_t pf_init_pose_mean = pf_vector_zero();
  pf_init_pose_mean.v[0] = init_pose_[0];
  pf_init_pose_mean.v[1] = init_pose_[1];
  pf_init_pose_mean.v[2] = init_pose_[2];
  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
  pf_init_pose_cov.m[0][0] = init_cov_[0];
  pf_init_pose_cov.m[1][1] = init_cov_[1];
  pf_init_pose_cov.m[2][2] = init_cov_[2];
  pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
  pf_init_ = false;

  // Instantiate the sensor objects
  // Odometry
  delete odom_;
  odom_ = new AMCLOdom();
  ROS_ASSERT(odom_);
  odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
  // Laser
  delete laser_;
  laser_ = new AMCLLaser(max_beams_, map_);
  ROS_ASSERT(laser_);
  if(laser_model_type_ == LASER_MODEL_BEAM)
    laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,
                         sigma_hit_, lambda_short_, 0.0);
  else if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD_PROB){
    ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
    laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_,
					laser_likelihood_max_dist_, 
					do_beamskip_, beam_skip_distance_, 
					beam_skip_threshold_, beam_skip_error_threshold_);
    ROS_INFO("Done initializing likelihood field model.");
  }
  else
  {
    ROS_INFO("Initializing likelihood field model; this can take some time on large maps...");
    laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
                                    laser_likelihood_max_dist_);
    ROS_INFO("Done initializing likelihood field model.");
  }

  // In case the initial pose message arrived before the first map,
  // try to apply the initial pose now that the map has arrived.
  applyInitialPose();

}

void
AmclNode::freeMapDependentMemory()
{
  if( map_ != NULL ) {
    map_free( map_ );
    map_ = NULL;
  }
  if( pf_ != NULL ) {
    pf_free( pf_ );
    pf_ = NULL;
  }
  delete odom_;
  odom_ = NULL;
  delete laser_;
  laser_ = NULL;
}

/**
 * Convert an OccupancyGrid map message into the internal
 * representation.  This allocates a map_t and returns it.
 */
map_t*
AmclNode::convertMap( const nav_msgs::OccupancyGrid& map_msg )
{
  map_t* map = map_alloc();
  ROS_ASSERT(map);

  map->size_x = map_msg.info.width;
  map->size_y = map_msg.info.height;
  map->scale = map_msg.info.resolution;
  map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
  map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;
  // Convert to player format
  map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
  ROS_ASSERT(map->cells);
  for(int i=0;i<map->size_x * map->size_y;i++)
  {
    if(map_msg.data[i] == 0)
      map->cells[i].occ_state = -1;
    else if(map_msg.data[i] == 100)
      map->cells[i].occ_state = +1;
    else
      map->cells[i].occ_state = 0;
  }

  return map;
}

AmclNode::~AmclNode()
{
  delete dsrv_;
  freeMapDependentMemory();
  delete laser_scan_filter_;
  delete laser_scan_sub_;
  delete tfb_;
  delete tf_;
  // TODO: delete everything allocated in constructor
}

bool AmclNode::isAmclPositionInitialOk()
{
    if(0==initialAgvPositionCount)
    {
        return true;
    }
    return false;
}

int AmclNode::getAmclDebugInfo(AmclDebugInfo &debugInfoOut)
{
    debugInfoOut=debugInfo;
    return 1;
}

int AmclNode::notifyReflectPositionInfo(PositionInfo vehiclePositionIn)
{
    usingMutex.lock();
    reflectPosition=vehiclePositionIn;
    usingMutex.unlock();
    return 1;
}

int AmclNode::setAmclPara(AmclDebugPara paraIn)
{
    qDebug()<<"AmclNode::setAmclPara";
    configParameter->writeAmclPara(paraIn);
    boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);

    max_beams_=paraIn.laser_max_beams;
    max_particles_=paraIn.max_particles;
    min_particles_=paraIn.min_particles;
    a_thresh_=paraIn.update_min_a;
    d_thresh_=paraIn.update_min_d;
    alpha1_=paraIn.odom_alpha1;
    alpha2_=paraIn.odom_alpha2;
    alpha3_=paraIn.odom_alpha3;
    alpha4_=paraIn.odom_alpha4;
    alpha5_=paraIn.odom_alpha5;
    configParameter->odom_alpha1=paraIn.odom_alpha1;
    configParameter->odom_alpha2=paraIn.odom_alpha2;
    configParameter->odom_alpha3=paraIn.odom_alpha3;
    configParameter->odom_alpha4=paraIn.odom_alpha4;
    configParameter->odom_alpha5=paraIn.odom_alpha5;
    calculateTimesForLaser=paraIn.calculateTimesForLaser;

    goodMatchScore=paraIn.goodMatchScore;
    badMatchScore=paraIn.badMatchScore;
    newOdom_alpha_badMatchScore=paraIn.newOdom_alpha_badMatchScore;
    alpha_slow_=paraIn.recovery_alpha_slow;
    alpha_fast_=paraIn.recovery_alpha_fast;
    resample_interval_=paraIn.resample_interval;
    odomOffsetFilterCount=paraIn.odomOffsetFilterCount;
    isCorrectByReflect=paraIn.isCorrectByReflect;
    if(correctFilterCount>=1)
    {
       correctFilterCount=paraIn.correctFilterCount;
    }
    qualifyReflectMinCertaintyScore=paraIn.qualifyReflectMinCertaintyScore;
    correctAllowMaxDistanceOffset=paraIn.correctAllowMaxDistanceOffset;
    correctAllowMaxAngleOffset=paraIn.correctAllowMaxAngleOffset;

    //pf
    pf_ = pf_alloc(min_particles_, max_particles_,
                   alpha_slow_, alpha_fast_,
                   (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
                   (void *)map_);
    pf_->pop_err = pf_err_;
    pf_->pop_z = pf_z_;

    // Initialize the filter
    pf_vector_t pf_init_pose_mean = pf_vector_zero();
    pf_init_pose_mean.v[0] = last_published_pose.pose.pose.position.x;
    pf_init_pose_mean.v[1] = last_published_pose.pose.pose.position.y;
    pf_init_pose_mean.v[2] = tf::getYaw(last_published_pose.pose.pose.orientation);
    pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
    pf_init_pose_cov.m[0][0] = last_published_pose.pose.covariance[6*0+0];
    pf_init_pose_cov.m[1][1] = last_published_pose.pose.covariance[6*1+1];
    pf_init_pose_cov.m[2][2] = last_published_pose.pose.covariance[6*5+5];
    pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
    pf_init_ = false;

    //odom
    delete odom_;
    odom_ = new AMCLOdom();
    ROS_ASSERT(odom_);
    odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );

    if(100==goodMatchScore)
    {
        debugFlag=1;
    }
    else
    {
        debugFlag=0;
    }

    return 1;
}

int AmclNode::getAmclPara(AmclDebugPara &paraOut)
{
    qDebug()<<"AmclNode::getAmclPara";
    paraOut.laser_max_beams=max_beams_;
    paraOut.max_particles=max_particles_;
    paraOut.min_particles=min_particles_;
    paraOut.update_min_a=a_thresh_;
    paraOut.update_min_d=d_thresh_;
    paraOut.odom_alpha1=configParameter->odom_alpha1;
    paraOut.odom_alpha2=configParameter->odom_alpha2;
    paraOut.odom_alpha3=configParameter->odom_alpha3;
    paraOut.odom_alpha4=configParameter->odom_alpha4;
    paraOut.odom_alpha5=configParameter->odom_alpha5;
    paraOut.calculateTimesForLaser=calculateTimesForLaser;

    paraOut.badMatchScore=badMatchScore;
    paraOut.goodMatchScore=goodMatchScore;
    paraOut.newOdom_alpha_badMatchScore=newOdom_alpha_badMatchScore;
    paraOut.recovery_alpha_slow=alpha_slow_;
    paraOut.recovery_alpha_fast=alpha_fast_;
    paraOut.resample_interval=resample_interval_;
    paraOut.odomOffsetFilterCount=odomOffsetFilterCount;

    paraOut.isCorrectByReflect=isCorrectByReflect;
    paraOut.qualifyReflectMinCertaintyScore=qualifyReflectMinCertaintyScore;
    paraOut.correctFilterCount=correctFilterCount;
    paraOut.correctAllowMaxDistanceOffset=correctAllowMaxDistanceOffset;
    paraOut.correctAllowMaxAngleOffset=correctAllowMaxAngleOffset;
    return 1;
}

int AmclNode::forceInitialAgvPosition(int calculateTime)
{
    qDebug()<<"AmclNode::forceInitialAgvPosition"<<calculateTime;
      initialAgvPositionCount=calculateTime;
      return 1;
}

int AmclNode::getVehiclePositionFrom_amcl(PositionInfo &positionInfoOut)
{

//    if(isCorrectByReflect)
//    {
//        if(1==calculateCorrectPosition(reflectPosition,amclPosition.position.x
//                                    ,amclPosition.position.y,
//                                    amclPosition.position.rotate))
//        {
//            //求校正矩阵.
////            usingMutex.lock();
////            calculateOffsetMatrix(amclPosition.position,frame_baselink_in_odom,frame_odom_in_map);
////            usingMutex.unlock();


////            calculateAgvPosition_byOdom(amclPosition);
////            sendOdomToMapTf(amclPosition.position);//单独屏蔽还是旋转时位置乱．
//            // 重新分布粒子，使其收缩.
//            setCorrectPose(amclPosition.position);//单独屏蔽还是旋转时位置乱．


//        }
//    }


    calculateAgvPosition_byOdom(amclPosition);
    positionInfoOut=amclPosition;
    return 1;
}


int AmclNode::calculateAgvPosition_byOdom(PositionInfo &vehiclePositionInfoOut)
{
    usingMutex.lock();
      //            agvInMapFrame=frame_odom_in_map*frame_baselink_in_odom;
    KDL::Frame agvInMapFrame=(*frame_odom_in_map)*(*frame_baselink_in_odom);
    vehiclePositionInfoOut.isUpdateSucess=isAmclPositionInitialOk();
    vehiclePositionInfoOut.maxMatchError=10.0/positionCertaintyFactor;
    vehiclePositionInfoOut.matchScore=positionCertaintyFactor;
    vehiclePositionInfoOut.position.x=agvInMapFrame.p[0];
    vehiclePositionInfoOut.position.y=agvInMapFrame.p[1];

    double tmp_roll,tmp_pitch,tmp_yaw;
    agvInMapFrame.M.GetRPY(tmp_roll,tmp_pitch,tmp_yaw);
    vehiclePositionInfoOut.position.rotate=tmp_yaw;

    usingMutex.unlock();
    return 1;

}

int AmclNode::setLaserLinkInBaseLink(double x, double y, double z, double roll, double pitch, double yaw)
{
    KDL::Rotation tmpRotation;
    tmpRotation=tmpRotation.RPY(roll,pitch,yaw);
//    laser_baslink_pitch=pitch;
    KDL::Vector tmpVector(x,y,z);
    *frame_laserlink_in_baselink =KDL::Frame(tmpRotation,tmpVector);
    *frame_baselink_in_laserlink=frame_laserlink_in_baselink->Inverse();

    return 1;
}

int AmclNode::setWheelOdometerData_noRos(OdemeterData odemeterDataIn)
{
    usingMutex.lock();

    odemeterData=odemeterDataIn;
    frame_baselink_in_odom->p[0]=odemeterDataIn.pos.x;
    frame_baselink_in_odom->p[1]=odemeterDataIn.pos.y;
    frame_baselink_in_odom->p[2]=0;
    frame_baselink_in_odom->M=frame_baselink_in_odom->M.RPY(0,0,odemeterDataIn.pos.rotate);
    usingMutex.unlock();
    return 1;
}

int AmclNode::getInitialPose(double &xOut, double &yOut, double &rOut)
{
    xOut=init_pose_[0];
    yOut=init_pose_[1];
    rOut=init_pose_[2];
    return 1;
}

int AmclNode::setAmclIsCalculateLaser(bool isCalculateLaserIn)
{
    isCalculateLaser=isCalculateLaserIn;
    return 1;
}
//boost::shared_ptr是可以共享所有权的指针。如果有多个shared_ptr共同管理同一个对象时，
//只有这些shared_ptr全部与该对象脱离关系之后，被管理的对象才会被释放。
//避免对shared_ptr所管理的对象的直接内存管理操作，以免造成该对象的重释放。
//shared_ptr并不能对循环引用的对象内存自动管理（这点是其它各种引用计数管理内存方式的通病）。
//不要构造一个临时的shared_ptr作为函数的参数（有内存泄露风险，取决于编译器厂商的实现）。
// boost::shared_ptr<Book> myBook(new Book("「1984」"));使用ｎｅｗ构造对象
// boost::shared_ptr是线程安全的，所以可能有锁等待的问题。

int AmclNode::setLaserSensorData(const sensor_msgs::LaserScanConstPtr &laserSensorDataIn)
{
//    if(false==isUseRosLaserMsg)
//    {
        last_laser_received_ts_ = ros::Time::now();
        laser_scan_buffer.reset();
        laser_scan_buffer=laserSensorDataIn;
        isLaserReadyForCalculate=true;
        return 1;
//    }
//    return -1;
}

int AmclNode::requestCountPlus()
{
    if(debugFlag)
    {
        qDebug()<<"AmclNode::requestCountPlus()0";
    }
    //AMCL第一次读地图时，初始化时间会花掉好几秒钟。切换地图可能花费时间较长。
    if(true==isEnableToWork )
    {
        if(debugFlag)
        {
            qDebug()<<"AmclNode::requestCountPlus() isEnableToWork";
        }
        if( noCheckCount<=0)
        {
            extRequestCount++;
            laserRequestCount++;
        }
        else
        {
            noCheckCount--;
        }



        //if(1!=laserDataCalculate(laser_scan_buffer))
        usingMutex.lock();//
        frame_baselink_in_odom_old=frame_baselink_in_odom;
        usingMutex.unlock();
        bool isResetCloudOut;
        int tmpResult=laserDataCalculate_multicalculate(laser_scan_buffer,isResetCloudOut);
        if(tmpResult>0)
        {
//            qDebug()<<"calculateOffsetMatrix";
            //求校正矩阵.地图接收完成后也需要计算一次．
            VehiclePosition tmpPosition;
            tmpPosition.x=last_published_pose.pose.pose.position.x;
            tmpPosition.y=last_published_pose.pose.pose.position.y;
            tmpPosition.rotate=tf::getYaw(last_published_pose.pose.pose.orientation);

//            usingMutex.lock();//
//            calculateOffsetMatrix(tmpPosition,frame_baselink_in_odom_old,frame_odom_in_map);
//            usingMutex.unlock();

            PositionInfo vehiclePositionInfoOut;
            calculateAgvPosition_byOdom(vehiclePositionInfoOut);
//            qDebug()<<"last_published_pose"<<tmpPosition.x<<tmpPosition.y;
//            qDebug()<<"calculateAgvPosition_byOdom"<<vehiclePositionInfoOut.position.x<<vehiclePositionInfoOut.position.y;
            // 重新分布粒子，使其收缩
            if(false==force_publication && true==isResetCloudOut)
            {
//                setCorrectPose(vehiclePositionInfoOut.position,true);
            }

//            if(isCorrectByReflect)
//            {
//                if(1==debugInfo.isCorrectSucess)
//                {
//                    setCorrectPose(vehiclePositionInfoOut.position);
//                }

//            }

        }
        else
        {
//            //没有收到激光数据也要发送tf，否则tf停止了。已经发送过，才可以发送旧的。
//            // We want to send a transform that is good up until a
//            // tolerance time so that odom can be used
//            if(true== sent_first_transform_ )//太耗时间,不发布rviz显示不正常
//            {
//                ros::Time transform_expiration = (ros::Time::now() +transform_tolerance_);
//                tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
//                                                    transform_expiration,
//                                                    global_frame_id_, odom_frame_id_);
//                this->tfb_->sendTransform(tmp_tf_stamped);
//            }

        }


    }

    return 1;
}

int AmclNode::calculateOffsetMatrix(VehiclePosition agvInMapPositionIn, KDL::Frame *frame_baselink_in_odomIn,
                                          KDL::Frame *frame_odom_in_mapOut)
{
//    //agvInMapFrame
//    KDL::Frame agvInMapFrame;
//    agvInMapFrame.p.data[0]=agvInMapPositionIn.x;
//    agvInMapFrame.p.data[1]=agvInMapPositionIn.y;
//    agvInMapFrame.p.data[2]=0;
//    agvInMapFrame.M=agvInMapFrame.M.RPY(0,0,agvInMapPositionIn.rotate);


//    //            agvInMapFrame=frame_odom_in_map*frame_baselink_in_odom;
//    *frame_odom_in_mapOut=agvInMapFrame*frame_baselink_in_odomIn->Inverse();
//    return 1;

    //agvInMapFrame
    KDL::Frame agvInMapFrame;
    agvInMapFrame.p.data[0]=agvInMapPositionIn.x;
    agvInMapFrame.p.data[1]=agvInMapPositionIn.y;
    agvInMapFrame.p.data[2]=0;
    agvInMapFrame.M=agvInMapFrame.M.RPY(0,0,agvInMapPositionIn.rotate);

    //            agvInMapFrame=frame_odom_in_map*frame_baselink_in_odom;
    KDL::Frame tmpOffsetFrame;
    tmpOffsetFrame=agvInMapFrame*frame_baselink_in_odomIn->Inverse();

    //对校准偏差做滤波处理．sick和r2000,静止时的位置变换范围都较大．
    double tmpOffsetX,tmpOffsetY,tmpOffsetR;
    tmpOffsetX=tmpOffsetFrame.p[0];
    tmpOffsetY=tmpOffsetFrame.p[1];
    double tmp_roll,tmp_pitch,tmp_yaw;
    tmpOffsetFrame.M.GetRPY(tmp_roll,tmp_pitch,tmp_yaw);
    tmpOffsetR=tmp_yaw;
    if(initialPositionCount>0)
    {
        filtering_position_x->resetAll();
        filtering_position_y->resetAll();
        filtering_position_r->resetAll();
        initialPositionCount--;
    }


    //arithmeticMeanfilter,medianMean 好像位置异常．原地旋转时，如果更新频率太快，滤波长度过短３，可能导致控制失去稳定性．
    double odomOffset_x=filtering_position_x->arithmeticMeanfilter(tmpOffsetX,odomOffsetFilterCount);
    double odomOffset_y=filtering_position_y->arithmeticMeanfilter(tmpOffsetY,odomOffsetFilterCount);
    double odomOffset_r=filtering_position_r->arithmeticMeanfilter_angle(tmpOffsetR,odomOffsetFilterCount);

//    qDebug()<<"tmpOffsetX"<<tmpOffsetX<<"tmpOffsetY"<<tmpOffsetY<<"tmpOffsetR"<<tmpOffsetR
//            <<"odomOffset_x"<<odomOffset_x<<"odomOffset_y"<<odomOffset_y<<"odomOffset_r"<<odomOffset_r;

    (*frame_odom_in_mapOut).p.data[0]=odomOffset_x;
    (*frame_odom_in_mapOut).p.data[1]=odomOffset_y;
    (*frame_odom_in_mapOut).p.data[2]=0;
    (*frame_odom_in_mapOut).M=(*frame_odom_in_mapOut).M.RPY(0,0,odomOffset_r);


    return 1;
}

int AmclNode::calculateOffsetMatrix_tf(tf::Stamped<tf::Pose> odom_to_mapIn,KDL::Frame *frame_odom_in_mapOut)
{


    //对校准偏差做滤波处理．sick和r2000,静止时的位置变换范围都较大．
    double tmpOffsetX,tmpOffsetY,tmpOffsetR;
    tmpOffsetX=odom_to_mapIn.getOrigin().getX();
    tmpOffsetY=odom_to_mapIn.getOrigin().getY();
    tmpOffsetR=tf::getYaw(odom_to_mapIn.getRotation());


    if(initialPositionCount>0)
    {
        filtering_position_x->resetAll();
        filtering_position_y->resetAll();
        filtering_position_r->resetAll();
        initialPositionCount--;
    }


    //arithmeticMeanfilter,medianMean 好像位置异常．原地旋转时，如果更新频率太快，滤波长度过短３，可能导致控制失去稳定性．
    double odomOffset_x=filtering_position_x->arithmeticMeanfilter(tmpOffsetX,odomOffsetFilterCount);
    double odomOffset_y=filtering_position_y->arithmeticMeanfilter(tmpOffsetY,odomOffsetFilterCount);
    double odomOffset_r=filtering_position_r->arithmeticMeanfilter_angle(tmpOffsetR,odomOffsetFilterCount);

    if(debugFlag)
    {
//            qDebug()<<"amclcalculateOffsetMatrix_tf:tmpOffsetX"<<tmpOffsetX<<"tmpOffsetY"<<tmpOffsetY<<"tmpOffsetR"<<tmpOffsetR
//                    <<"odomOffset_x"<<odomOffset_x<<"odomOffset_y"<<odomOffset_y<<"odomOffset_r"<<odomOffset_r;

    }

    (*frame_odom_in_mapOut).p.data[0]=odomOffset_x;
    (*frame_odom_in_mapOut).p.data[1]=odomOffset_y;
    (*frame_odom_in_mapOut).p.data[2]=0;
    (*frame_odom_in_mapOut).M=(*frame_odom_in_mapOut).M.RPY(0,0,odomOffset_r);

    *frame_odom_in_mapOut=frame_odom_in_mapOut->Inverse();


    return 1;
}

int AmclNode::getRequestCount()
{
    return extRequestCount;
}

int AmclNode::setNoCheckCount(int countIn)
{
    noCheckCount=countIn;
    return 1;

}

int AmclNode::getLaserRequestCount()
{
    return laserRequestCount;
}

int AmclNode::setVehicleVelocityFeedback(double lineSpeed, double rotateSpeed)
{
    //
//    qDebug()<<"AmclNode::setVehicleVelocityFeedback lineSpeed"<<lineSpeed<<"rotateSpeed"<<rotateSpeed;
    double newDistance;
    double tmpCountCalculate;
    if(lineSpeed<0.05)
    {
        newDistance=0.025;
//        tmpCountCalculate=10;
//        tmpCountCalculate=configParameterIn->calculateTimesForLaser*2;
    }
    else if(lineSpeed<0.1)
    {
        newDistance=0.036;
//        tmpCountCalculate=9;
//        tmpCountCalculate=configParameterIn->calculateTimesForLaser*1.8;
    }
    else if(lineSpeed<0.2)
    {
        newDistance=0.045;
//        tmpCountCalculate=8;
//        tmpCountCalculate=configParameterIn->calculateTimesForLaser*1.5;
    }
    else if(lineSpeed<0.3)
    {
        newDistance=0.05;
//        tmpCountCalculate=6;
//        tmpCountCalculate=configParameterIn->calculateTimesForLaser;
    }
    else if(lineSpeed<0.4)
    {
        newDistance=0.07;
//        tmpCountCalculate=5;
//        tmpCountCalculate=configParameterIn->calculateTimesForLaser*0.8;
    }
    else if(lineSpeed<0.5)
    {
        newDistance=0.09;
//        tmpCountCalculate=3;
//        tmpCountCalculate=configParameterIn->calculateTimesForLaser*0.5;
    }
    else if(lineSpeed<0.6)
    {
        newDistance=0.12;
//        tmpCountCalculate=2;
//        tmpCountCalculate=configParameterIn->calculateTimesForLaser*0.3;
    }
    else
    {
        newDistance=0.2;
//        tmpCountCalculate=1;
//        tmpCountCalculate=configParameterIn->calculateTimesForLaser*0.2;
    }
    if(tmpCountCalculate<1)
    {
//        tmpCountCalculate=1;

    }
//    calculateTimesForLaser=tmpCountCalculate;//容易出现偏差过大．匹配分数不高的时候
//    setDeltaDistanceUpdate(newDistance);

    //
    double newAngle;
    if(rotateSpeed<0.10)//0.1rad=5.7
    {
        newAngle=0.05;
    }
    else if(rotateSpeed<0.3)
    {
        newAngle=configParameter->update_min_a;//0.15rad=8.5degree
    }
    else if(rotateSpeed<0.6)
    {
        newAngle=2*configParameter->update_min_a;
    }
    else
    {
        newAngle=3*configParameter->update_min_a;
    }
//    setDeltaAngleUpdate(newAngle);

    return 1;

}

int AmclNode::setWorkStatus(bool isEnable)
{
    qDebug()<<"AmclNode::setWorkStatus"<<isEnable;
    isEnableToWork=isEnable;
    return 1;
}

int AmclNode::setUpdatePartialEveryTime(bool isUpdateIn)
{
    isUpdatePartialEveryTime=isUpdateIn;
    return 1;
}

int AmclNode::getPositionCertaintyFactor(double &certaintyFactorOut,float &weight_out,
                                         std::vector<float> &pos_cov_out)
{
    certaintyFactorOut=positionCertaintyFactor;

    weight_out=socre_weight;
    pos_cov_out.resize(9);
    for(int i=0;i<9;i++)
    {
        pos_cov_out[i]=socre_pos_cov[i];
    }

    return 1;
}

int AmclNode::getTimeConsumed(double &minConsumedOut, double &currentConsumedOut, double &maxConsumedOut)
{
    minConsumedOut=minConsumed;
    currentConsumedOut=currentConsumed;
    maxConsumedOut=maxConsumed;
    return 1;
}

int AmclNode::resetRosMeasureTime()
{
    minConsumed=100000;
    maxConsumed=-100000;
    return 1;
}

bool
AmclNode::getOdomPose(tf::Stamped<tf::Pose>& odom_pose,
                      double& x, double& y, double& yaw,
                      const ros::Time& t, const std::string& f)
{
  // Get the robot's pose
  tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
                                           tf::Vector3(0,0,0)), t, f);
  try
  {
    this->tf_->transformPose(odom_frame_id_, ident, odom_pose);
  }
  catch(tf::TransformException e)
  {
    ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
    return false;
  }
  x = odom_pose.getOrigin().x();
  y = odom_pose.getOrigin().y();
  double pitch,roll;
  odom_pose.getBasis().getEulerYPR(yaw, pitch, roll);

  return true;
}


pf_vector_t
AmclNode::uniformPoseGenerator(void* arg)
{
  map_t* map = (map_t*)arg;
#if NEW_UNIFORM_SAMPLING
  unsigned int rand_index = drand48() * free_space_indices.size();
  std::pair<int,int> free_point = free_space_indices[rand_index];
  pf_vector_t p;
  p.v[0] = MAP_WXGX(map, free_point.first);
  p.v[1] = MAP_WYGY(map, free_point.second);
  p.v[2] = drand48() * 2 * M_PI - M_PI;
#else
  double min_x, max_x, min_y, max_y;

  min_x = (map->size_x * map->scale)/2.0 - map->origin_x;
  max_x = (map->size_x * map->scale)/2.0 + map->origin_x;
  min_y = (map->size_y * map->scale)/2.0 - map->origin_y;
  max_y = (map->size_y * map->scale)/2.0 + map->origin_y;

  pf_vector_t p;

  ROS_DEBUG("Generating new uniform sample");
  for(;;)
  {
    p.v[0] = min_x + drand48() * (max_x - min_x);
    p.v[1] = min_y + drand48() * (max_y - min_y);
    p.v[2] = drand48() * 2 * M_PI - M_PI;
    // Check that it's a free cell
    int i,j;
    i = MAP_GXWX(map, p.v[0]);
    j = MAP_GYWY(map, p.v[1]);
    if(MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1))
      break;
  }
#endif
  return p;
}

bool
AmclNode::globalLocalizationCallback(std_srvs::Empty::Request& req,
                                     std_srvs::Empty::Response& res)
{
  if( map_ == NULL ) {
    return true;
  }
  boost::recursive_mutex::scoped_lock gl(configuration_mutex_);
  ROS_INFO("Initializing with uniform distribution");
  pf_init_model(pf_, (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,
                (void *)map_);
  ROS_INFO("Global initialisation done!");
  pf_init_ = false;
  return true;
}

// force nomotion updates (amcl updating without requiring motion)
bool 
AmclNode::nomotionUpdateCallback(std_srvs::Empty::Request& req,
                                     std_srvs::Empty::Response& res)
{
	m_force_update = true;
	//ROS_INFO("Requesting no-motion update");
	return true;
}

bool
AmclNode::setMapCallback(nav_msgs::SetMap::Request& req,
                         nav_msgs::SetMap::Response& res)
{
  handleMapMessage(req.map);
  handleInitialPoseMessage(req.initial_pose);
  res.success = true;
  return true;
}

void
AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
//    static int tick=0;
//    tick++;
//    qDebug()<<"AmclNode::laserReceived tick"<<tick;
    if(true==isUseRosLaserMsg)
    {
        last_laser_received_ts_ = ros::Time::now();
        laser_scan_buffer.reset();
        laser_scan_buffer=laser_scan;
        isLaserReadyForCalculate=true;
    }

//    qDebug()<<"laser_scan_buffer count"<<laser_scan_buffer.use_count();

}

double
AmclNode::getYaw(tf::Pose& t)
{
  double yaw, pitch, roll;
  t.getBasis().getEulerYPR(yaw,pitch,roll);
  return yaw;
}

void
AmclNode::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
  handleInitialPoseMessage(*msg);
}

void
AmclNode::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg)
{

    initialPositionCount=D_AMCL_POSITION_INITIAL_COUNT;
    filtering_position_x->resetAll();
    filtering_position_y->resetAll();
    filtering_position_r->resetAll();

  boost::recursive_mutex::scoped_lock prl(configuration_mutex_);
  if(msg.header.frame_id == "")
  {
    // This should be removed at some point
    ROS_WARN("Received initial pose with empty frame_id.  You should always supply a frame_id.");
  }
  // We only accept initial pose estimates in the global frame, #5148.
  else if(tf_->resolve(msg.header.frame_id) != tf_->resolve(global_frame_id_))
  {
    ROS_WARN("Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"",
             msg.header.frame_id.c_str(),
             global_frame_id_.c_str());
    return;
  }

  // In case the client sent us a pose estimate in the past, integrate the
  // intervening odometric change.
  tf::StampedTransform tx_odom;
  try
  {
    ros::Time now = ros::Time::now();
    // wait a little for the latest tf to become available
    tf_->waitForTransform(base_frame_id_, msg.header.stamp,
                         base_frame_id_, now,
                         odom_frame_id_, ros::Duration(0.5));
    tf_->lookupTransform(base_frame_id_, msg.header.stamp,
                         base_frame_id_, now,
                         odom_frame_id_, tx_odom);
  }
  catch(tf::TransformException e)
  {
    // If we've never sent a transform, then this is normal, because the
    // global_frame_id_ frame doesn't exist.  We only care about in-time
    // transformation for on-the-move pose-setting, so ignoring this
    // startup condition doesn't really cost us anything.
    if(sent_first_transform_)
      ROS_WARN("Failed to transform initial pose in time (%s)", e.what());
    tx_odom.setIdentity();
  }

  tf::Pose pose_old, pose_new;
  tf::poseMsgToTF(msg.pose.pose, pose_old);
  pose_new = pose_old * tx_odom;

  // Transform into the global frame

  ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f",
           ros::Time::now().toSec(),
           pose_new.getOrigin().x(),
           pose_new.getOrigin().y(),
           getYaw(pose_new));
  // Re-initialize the filter
  pf_vector_t pf_init_pose_mean = pf_vector_zero();
  pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
  pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
  pf_init_pose_mean.v[2] = getYaw(pose_new);
  pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
  // Copy in the covariance, converting from 6-D to 3-D
  for(int i=0; i<2; i++)
  {
    for(int j=0; j<2; j++)
    {
      pf_init_pose_cov.m[i][j] = msg.pose.covariance[6*i+j];
    }
  }
  pf_init_pose_cov.m[2][2] = msg.pose.covariance[6*5+5];

//  for(int i=0; i<36; i++)
//  {
//      qDebug()<<"msg.pose.covariance i"<<i<<msg.pose.covariance[i];
//  }

  delete initial_pose_hyp_;
  initial_pose_hyp_ = new amcl_hyp_t();
  initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
  initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
  applyInitialPose();

  qDebug()<<"AmclNode::handleInitialPoseMessage"<<pose_new.getOrigin().x()<<pose_new.getOrigin().y()
         <<getYaw(pose_new);
}

void AmclNode::setCorrectPose(VehiclePosition positionIn, bool pf_inital)
{
    qDebug()<<"777777777777777AmclNode::setCorrectPose";
     boost::recursive_mutex::scoped_lock prl(configuration_mutex_);
    // Re-initialize the filter
    pf_vector_t pf_init_pose_mean = pf_vector_zero();
    pf_init_pose_mean.v[0] = positionIn.x;
    pf_init_pose_mean.v[1] = positionIn.y;
    pf_init_pose_mean.v[2] = positionIn.rotate;
    pf_matrix_t pf_init_pose_cov = pf_matrix_zero();

    pf_init_pose_cov.m[0][0] = init_cov_[0];
    pf_init_pose_cov.m[1][1] = init_cov_[1];
    pf_init_pose_cov.m[2][2] = init_cov_[2];
//    // Copy in the covariance, converting from 6-D to 3-D
//    for(int i=0; i<2; i++)
//    {
//      for(int j=0; j<2; j++)
//      {
//        pf_init_pose_cov.m[i][j] = msg.pose.covariance[6*i+j];
//      }
//    }
//    pf_init_pose_cov.m[2][2] = msg.pose.covariance[6*5+5];

//    for(int i=0; i<36; i++)
//    {
//        qDebug()<<"msg.pose.covariance i"<<i<<msg.pose.covariance[i];
//    }

    delete initial_pose_hyp_;
    initial_pose_hyp_ = new amcl_hyp_t();
    initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
    initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
    applyInitialPose(pf_inital);
}

/**
 * If initial_pose_hyp_ and map_ are both non-null, apply the initial
 * pose to the particle filter state.  initial_pose_hyp_ is deleted
 * and set to NULL after it is used.
 */
void
AmclNode::applyInitialPose(bool pf_initIn)
{
  boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);
  if( initial_pose_hyp_ != NULL && map_ != NULL ) {
    pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov);
    if(false==pf_initIn)
    {
        pf_init_ = false;
    }


    delete initial_pose_hyp_;
    initial_pose_hyp_ = NULL;
  }
}
